Dieser Artikel ist noch in Bearbeitung.
Inhaltsverzeichnis
Listrik01 - Ein selbstbalancierendes Elektroeinrad
Beim Listrik01 handelt es sich um einen selbstbalancierenden Elektroroller. Die Elektronik basiert auf den Veröffentlichungen des Rutscherle 2 [1]. Die Schaltung wurde verändert um I2C Gyro und Accelerometer anschließen zu können. Die Listing wurden außerdem umfangreicher kommentiert.
Antrieb
Der Prototyp fährt mit einem 24V / 500W Gleichstrommotor. Das Gefährt ist allerdings extrem koflastig, deshalb baue ich es um auf einen 48V/1000W BLDC Motor. Vom Hersteller in China direkt importiert. Das Bild oben zeigt den GS-Prototyp. Bei der BLDC Variante werden Batterie, Steuerung und Motor in die Felge integriert. Dies sorgt für einen niedrigen Schwerkunkt. Habe mir ein 18" Motorrad Hinterrad besorgt in das alles integriert wird.
Die Sensorik wurde um 3 weitere Hallsensoren erweitert um einen Trigger je 60 Grad elektrisch zu bekommen. Die Geschwindigkeit ist auf 20km/h begrenzt.
Akkus
Der Gleichstrom Prototyp fährt mit 2 Bleiakkus mit 12V/7AH in Reihe geschaltet. Sie sind für Motoren und Elektronik. Die Entscheidung für Bleiakkus wurde bewusst getroffen. Bei einem Prototyp sind Ergebnisse wichtiger als Eleganz.
Der BLDC Prototyp fährt mit 4 Bleiakkus mit 12V/7AH in Reihe geschaltet. Sie sind für Motoren und Elektronik.
Elektronik
Motorregler mit Atmega 168A sowie eine Hauptsteuerung mit einem Atmega 644PA. Ein Display von Display3000 als Tacho (und Datenlogger). Siehe Stückliste.
Software
Die Software ist in Bascom geschrieben. Zeitkritische Teile in Assembler. Siehe Link oben zum Rutscherle 2 [2].
- Quellcode
Motorcontroller
'******************Listrik01 Motorsteuerung, Version 01 **********************
'Es gibt keine Stromregelung, sondern eine Lageregelung im Hauptcontroller
'LED1 blinkt mit gültigem seriellen Telegramm
'LED2 blinkt wenn Zwangskommutierung 'LED2 dauerlicht wenn serielle Verbindung fehlt, d.h. Not Halt
'LED1 und LED2 blinken abwechselnd mit 1Hz für 3 Sekunden während CPU Anlauf
'LED1 und LED2 blinken mit 2Hz wenn Hallstecker nicht gesteckt ist. ' Ausserdem antwortet der Controller jetzt nicht auf Telegramme.
'LockBit, FuseBit , FuseBitHigh , FuseBitExtended '$prog &HFF , &HF9 , &HDF , &HF6 'Fusebits 168A CPU
'Use AMTEL Studio / Fuses Menu instead to set fusebits 'BOOTSZ = 1024W_1C00 'BOOTRST = unchecked 'RSTDISBL = unchecked 'DWEN = unchecked 'SPIEN = checked 'WDTON = unchecked 'EESAVE = unchecked 'BODLEVEL = unchecked 'CKDIV8 = unchecked 'CKOUT = unchecked 'SUT_CKSEL = EXTFSXTAL_1KCK_14CK_4MS1
$regfile = "m168def.dat" 'ATMEGA168A CPU $crystal = 11059200 $baud = 57600 $hwstack = 32 $swstack = 20 $framesize = 40
Const _pcicr = &B00000101 'Bit0 = PCIE0 = Port B0..B7
'Bit2 = PCIE2 = Port D0..D7
Const Numhall = 3 '<<<<<<<<<< TEST, Normalerweise = 4 Const Pclink = 1 '<<<<<<<<<< TEST, Normalerweise = 0, Sie Delphi
Declare Sub Setspeed Declare Sub Pwm_start Declare Sub Pwm_stop
Config Portb.0 = Input 'Hall_D Config Portb.1 = Output 'B- Config Portb.2 = Output 'B+ Config Portb.3 = Output 'A-(MOSI) Config Portb.4 = Output 'LED2 (MISO) Config Portb.5 = Input 'SCK ' PortB.6 XTAL ' PortB.7 XTAL
Config Portc.0 = Input 'UBatterie Config Portc.1 = Input 'I-Ist Config Portc.2 = Input 'Kühlkörper Temperatur Config Portc.3 = Output 'LED1 Config Portc.4 = Output 'Lüfter Config Portc.5 = Input 'Adress Jumper ' PortC.6 Reset ' PortC.7 nicht rausgeführt
' PortD.0 RxD ' PortD.1 TxD Config Portd.2 = Input 'Hall_C Config Portd.3 = Output 'A+ Config Portd.4 = Input 'Hall_B Config Portd.5 = Output 'C+ Config Portd.6 = Output 'C- Config Portd.7 = Input 'Hall_A
Hall_a Alias Pind.7 Hall_b Alias Pind.4 Hall_c Alias Pind.2 Hall_d Alias Pinb.0 Led1 Alias Portc.3 Led2 Alias Portb.4 Fan Alias Portc.4 Adrjumper Alias Pinc.5
'A-Werte stellen die Rotorposition in Abhängigkeit der Halleingänge dar 'Siehe Exceltabelle "Rotorposition Array A().xls" Dim A(15) As Byte At $100
'S-Werte stellen die aktuelle Soll PWM-Breite dar Dim S(12) As Byte At $10f
'0 = Vorwärts, 1 = Links, 2 = Leerlauf, 3 = Dim Richtung(255) As Byte At $11b
'IstRichtung-Werte stellen den 0..2, siehe Zeile oberhalb Dim Istrichtung As Integer At $21a
'Ticks-Werte stellen die Anzahl verbrauchter Ticks während eine Do..Loop dar Dim Ticks As Integer At $221
'Anzahl Kommutierungen zwischen seriellen Telegrammen Dim Uzu As Integer 'Netzspannung Dim M1 As Byte At Uzu + 0 Overlay Dim M2 As Byte At Uzu + 1 Overlay
Dim Izu As Integer 'Motorstrom Dim M3 As Byte At Izu + 0 Overlay Dim M4 As Byte At Izu + 1 Overlay
Dim Ausgabeticks As Integer 'Anzahl Ticks für ein Do ... Loop Dim M5 As Byte At Ausgabeticks + 0 Overlay Dim M6 As Byte At Ausgabeticks + 1 Overlay
Dim Temperatur As Integer 'Kühlkörpertemperatur Dim M7 As Byte At Temperatur + 0 Overlay Dim M8 As Byte At Temperatur + 1 Overlay
Dim Drehzahl As Integer 'Drehzahl Dim M9 As Byte At Drehzahl + 0 Overlay Dim M10 As Byte At Drehzahl + 1 Overlay
Dim Kalibrierwert As Integer Dim Kal(2) As Byte At Kalibrierwert + 0 Overlay
Dim Sinus(12) As Byte Dim Motorposition As Byte Dim Altrichtung As Integer Dim Aposition As Byte Dim Zposition As Byte Dim Zwangskommutieren As Byte Dim Temp As Word Dim Motoradresse As Byte Dim Vwinkel As Byte Dim Rwinkel As Byte Dim N As Byte Dim Notaus As Byte Dim Loopcount As Word Dim Sollrichtung As Byte Dim Altsollrichtung As Byte Dim Sollspeed As Byte Dim Altsollspeed As Byte Dim Bdummy As Byte Dim Intbdummy As Byte Dim Intidummy As Integer Dim Sdummy As Single Dim Idummy1 As Integer Dim Idummy2 As Integer Dim Uw As Word Dim Iw As Word Dim Uwsum As Long Dim Iwsum As Long Dim Wcount As Long Dim Fancount As Long Dim Uoffset As Single Dim Ufaktor As Single Dim Ioffset As Single Dim Ifaktor As Single Dim Empfpuffer(6) As Byte 'Empfangspuffer Dim Ta As Byte Dim Tb As Byte Dim Tc As Byte Dim Fehler As Byte
Dim Errorword As Word Dim Errorword1 As Byte At Errorword + 0 Overlay Dim Errorword2 As Byte At Errorword + 1 Overlay
'Integer bzw. Word (2Byte) = % 'Single (4Byte) = ! 'Double (8Byte) = # 'Long (4Byte) = & ==> 32Bit Integer 'Byte (1Byte) = ohne Zusatz
$eeprom 'wird nicht benutzt
U_offset: Data 0.0! 'Single U_faktor: Data 1.0! 'Single I_offset: Data 0.0! 'Single I_faktor: Data 1.0! 'Single V_winkel: Data 0 'Byte R_winkel: Data 0 'Byte Sinus_12: 'Sinus mit 4 Hallgebern Data 0 , 0 , 33 , 66 , 100 , 100 , 100 , 100 , 66 , 33 , 0 , 0 Sinus_6: 'Sinus mit 3 Hallgebern Data 0 , 33 , 66 , 100 , 66 , 33
$data
Sollspeed = 0 Pwm_stop 'Erstmal alles auschalten
For N = 1 To 15
A(n) = 0
Next
'Variablen für die Richtungserkennung vorbelegen 'Diese Variante frisst zwar viel Arbeitsspeicher, ist aber sehr schnell For N = 1 To 255
Richtung(n) = 2
Next
If Adrjumper = 0 Then 'Rechter Motor
Motoradresse = 82 #if Numhall = 3 'Anzahl Hallsensoren A(5) = 1 A(1) = 2 A(3) = 3 A(2) = 4 A(6) = 5 A(4) = 6 #else A(12) = 12 A(4) = 11 A(6) = 10 A(14) = 9 A(10) = 8 A(2) = 7 A(3) = 6 A(11) = 5 A(9) = 4 A(1) = 3 A(5) = 2 A(13) = 1 #endif
Richtung(21) = 3 Richtung(93) = 3 Richtung(220) = 3 Richtung(196) = 3 Richtung(70) = 3 Richtung(110) = 3 Richtung(234) = 3 Richtung(162) = 3 Richtung(35) = 3 Richtung(59) = 3 Richtung(185) = 3 Richtung(145) = 3 Richtung(81) = 1 Richtung(25) = 1 Richtung(155) = 1 Richtung(179) = 1 Richtung(50) = 1 Richtung(42) = 1 Richtung(174) = 1 Richtung(230) = 1 Richtung(100) = 1 Richtung(76) = 1 Richtung(205) = 1 Richtung(213) = 1
Else
Motoradresse = 76 'Linker Motor #if Numhall = 3 'Anzahl Hallsensoren A(3) = 1 A(1) = 2 A(5) = 3 A(4) = 4 A(6) = 5 A(2) = 6 #else A(11) = 1 A(3) = 2 A(1) = 3 A(9) = 4 A(13) = 5 A(5) = 6 A(4) = 7 A(12) = 8 A(14) = 9 A(6) = 10 A(2) = 11 A(10) = 12 #endif
Richtung(236) = 3 Richtung(196) = 3 Richtung(69) = 3 Richtung(93) = 3 Richtung(217) = 3 Richtung(145) = 3 Richtung(19) = 3 Richtung(59) = 3 Richtung(186) = 3 Richtung(162) = 3 Richtung(38) = 3 Richtung(110) = 3 Richtung(42) = 1 Richtung(171) = 1 Richtung(179) = 1 Richtung(49) = 1 Richtung(25) = 1 Richtung(157) = 1 Richtung(213) = 1 Richtung(84) = 1 Richtung(76) = 1 Richtung(206) = 1 Richtung(230) = 1 Richtung(98) = 1
End If
'AD-Wandler einstellen Config Adc = Single , Prescaler = Auto , Reference = Off Start Adc
'<Test Hallsensoren> Fehler = 1 Fan = 1 While Fehler = 1
Fehler = 0 Motorposition = 0 Motorposition.0 = Hall_a Motorposition.1 = Hall_b Motorposition.2 = Hall_c #if Numhall = 3 'Anzahl Hallsensoren Motorposition.3 = 0 #else Motorposition.3 = Hall_d #endif If Motorposition = 0 Then Fehler = 1 'Stecker nicht gesteckt If Motorposition > 0 Then 'Das währe eine Motorposition die es nicht gibt If A(motorposition) = 0 Then Fehler = 1 End If If Fehler = 1 Then Toggle Led1 Toggle Led2 Waitms 250 '==> 2Hz End If
Wend '</Test Hallsensoren>
On Timer2 Update_pwm Nosave
'Pin-Change Interrupt einstellen: (Hall Impulse) Pcmsk2 = &B10010100 'Hall A, B, C Pcmsk1 = &B00000000 Pcmsk0 = &B00000001 'Hall D Pcicr = _pcicr
'Wenn sich das Hallsignal ändert, wird kommutiert On Pcint2 Kommutieren Nosave On Pcint0 Kommutieren Nosave
'EEPROM mit dem Programm Motorcontroller_EEPROM.bas vorbesetzen ' Datenreihenfolge dort ist ab Byte(1): ' U_offset, U_faktor, I_offset, I_faktor im Single Format ' Voreilwinkel, Nacheilwinkel im Byte Format 'Sinuskurve 12 Werte im Byte Format für 4 Hall Sensoren 'Sinuskurve 6 Werte im Byte Format für 3 Hall Sensoren
'Readeeprom Uoffset , 1 Uoffset = 0
'Readeeprom Ufaktor , 5 Ufaktor = 5.37109375 'Siehe Hardwarezeichnung für Berechnung
'Readeeprom Ioffset , 9 Ioffset = 512 '2,5V
'Readeeprom Ifaktor , 13 Ifaktor = 7.8125 'Siehe Hardwarezeichnung für Berechnung
'Voreilwinkel: 'Readeeprom Vwinkel , 17 Vwinkel = 0
'Readeeprom Rwinkel , 18 Rwinkel = 0
If Ufaktor = 0 Then Ufaktor = 1.0 If Ifaktor = 0 Then Ifaktor = 1.0
'Angepasste Sinuskurve aus dem EEprom laden
- if Numhall = 3 'Anzahl Hallsensoren
' For N = 1 To 6 ' Bdummy = N + 30 ' Readeeprom Sinus(n) , Bdummy ' Next
Sinus(1) = 0 Sinus(2) = 33 Sinus(3) = 66 Sinus(4) = 100 Sinus(5) = 66 Sinus(6) = 33
- else 'Anzahl Hallsensoren = 4
' For N = 1 To 12 ' Bdummy = N + 18 ' Readeeprom Sinus(n) , Bdummy ' Next
Sinus(1) = 0 Sinus(2) = 0 Sinus(3) = 33 Sinus(4) = 66 Sinus(5) = 100 Sinus(6) = 100 Sinus(7) = 100 Sinus(8) = 100 Sinus(9) = 66 Sinus(10) = 33 Sinus(11) = 0 Sinus(12) = 0
- endif
Fan = 1 'Lüfter kurz einschalten damit die Lager nicht fest werden For N = 1 To 3 '3 sekunden
Led1 = 1 Led2 = 0 Waitms 500 Toggle Led1 Toggle Led2 Waitms 500
Next Led2 = 0 Fan = 0
On Urxc Datenempfang Nosave Enable Urxc 'serielles UART. No parity, 1 stop bit, 8 data bits Enable Interrupts
Do
Led1 = 0
'<Messen>
Uw = Getadc(0) + 14 'Versorgungsspannung. Siehe Hardwarezeichnung
Uwsum = Uwsum + Uw
Iw = Getadc(1) 'Gesamtstrom
Iwsum = Iwsum + Iw
Incr Wcount
'Kühkörpertemperatur in counts. Siehe Datenblätter\10K_NTC_Kennlinie.odt
Temp = Getadc(2)
'</Messen>
'<Serielle Telegramme abarbeiten>
' Empfpuffer(1) Empfpuffer(2) Empfpuffer(3) Empfpuffer(4) Empfpuffer(5) Empfpuffer(6)
' <CR> Contr.Adresse Richtung PWM Checksum <CR>
If Empfpuffer(1) = 13 Then 'Das letzte im Ring verschobene empfangene CR
If Empfpuffer(6) = 13 Then
Led1 = 1
If Empfpuffer(2) = Motoradresse Then '"R" oder "L"
Disable Urxc
'BCC für Empfangsmessage berechnen
!lds R24,{Empfpuffer(2)} 'Adresse. Load Direct from data space @address xxx
!LDs R25,{Empfpuffer(3)} 'Richtung.Load Direct from data space @address xxx
!add R24,R25
!LDs R25,{Empfpuffer(4)} 'PWM. Load Direct from data space @address xxx
!add R24,R25
!STS {Bdummy},R24 'Write Rxx to address loaction
If Bdummy = Empfpuffer(5) Then 'Die Prüfsumme stimmt
'Kommutieren deaktivieren, weil sonst die Ticks während dem Zugriff
'geändert werden könnten
Pcicr = &B00000000
Ausgabeticks = Ticks
Ticks = 0
Pcicr = _pcicr
'BCC für Sendemessage berechnen
!lds R24,{M1} 'Load Direct from data space @address xxx into Rxx
!LDs R25,{M2} 'Load Direct from data space @address xxx into Rxx
!add R24,R25 'Add Rxx + Ryy, Result in Rxx
!LDs R25,{M3} 'Load Direct from data space @address xxx into Rxx
!add R24,R25 'Add Rxx + Ryy, Result in Rxx
!LDs R25,{M4} 'Load Direct from data space @address xxx into Rxx
!add R24,R25 'Add Rxx + Ryy, Result in Rxx
!LDs R25,{M5} 'Load Direct from data space @address xxx into Rxx
!add R24,R25 'Add Rxx + Ryy, Result in Rxx
!LDs R25,{M6} 'Load Direct from data space @address xxx into Rxx
!add R24,R25 'Add Rxx + Ryy, Result in Rxx
!STS {Bdummy},R24 'Write Rxx to address loaction
'Antwort am UART ausgeben
'M1/M2=Spg M3/M4=Strom M5/M6=Loop Ticks Bdummy=Checksum
'Print M1 ; M2 ; M3 ; M4 ; M5 ; M6 ; Bdummy ; 13
'in Assembler geht das so...
!LdS R24,{M1} 'Load Direct from data space @address xxx into Rxx
!STS udr,R24 'Write Rxx to address loaction
Warten1:
!LDS R24,UCSR0A 'Load Direct from data space @address xxx into Rxx
!BST R24,5 'Store bit x of Rxx in T Flag
!Brtc warten1
!LDS R24,{M2} 'Load Direct from data space @address xxx into Rxx
!STS udr,R24 'Write Rxx to address loaction
Warten2:
!LDS R24,UCSR0A 'Load Direct from data space @address xxx into Rxx
!BST R24,5 'Store bit x of Rxx in T Flag
!Brtc warten2
!lds R24,{M3} 'Load Direct from data space @address xxx into Rxx
!STS udr,R24 'Write Rxx to address loaction
Warten3:
!LDS R24,UCSR0A 'Load Direct from data space @address xxx into Rxx
!BST R24,5 'Store bit x of Rxx in T Flag
!Brtc warten3
!LDS R24,{M4} 'Load Direct from data space @address xxx into Rxx
!STS udr,R24 'Write Rxx to address loaction
Warten4:
!LDS R24,UCSR0A 'Load Direct from data space @address xxx into Rxx
!BST R24,5 'Store bit x of Rxx in T Flag
!Brtc warten4
!LDS R24,{M5} 'Load Direct from data space @address xxx into Rxx
!STS udr,R24 'Write Rxx to address loaction
Warten5:
!LDS R24,UCSR0A 'Load Direct from data space @address xxx into Rxx
!BST R24,5 'Store bit x of Rxx in T Flag
!Brtc warten5
!LDS R24,{M6} 'Load Direct from data space @address xxx into Rxx
!STS udr,R24 'Write Rxx to address loaction
Warten6:
!LDS R24,UCSR0A 'Load Direct from data space @address xxx into Rxx
!BST R24,5 'Store bit x of Rxx in T Flag
!Brtc warten6
!LDS R24,{bdummy}
!STS udr,R24 'Write Rxx to address loaction
Warten7:
!LDS R24,UCSR0A 'Load Direct from data space @address xxx into Rxx
!BST R24,5 'Store bit x of Rxx in T Flag
!Brtc warten7
!LDi R24,13
!STS udr,R24 'Write Rxx to address loaction
Warten8:
!LDS R24,UCSR0A 'Load Direct from data space @address xxx into Rxx
!BST R24,5 'Store bit x of Rxx in T Flag
!Brtc warten8
If Empfpuffer(3) < 3 Then 'Richtung
Notaus = 0
Loopcount = 0
Sollrichtung = Empfpuffer(3) 'Richtung
Sollspeed = Empfpuffer(4) 'PWM Breite
If Sollrichtung = 2 Then
Sollrichtung = 1
Notaus = 1
Loopcount = 250
Sollspeed = 0
End If
Setspeed
'<Messwerte berechnen>
'Messwerte werden hier berechnet, weil genau jetzt 1/100s Zeit ist.
'Spannung berechnen
Sdummy = Uwsum / Wcount
Sdummy = Sdummy - Uoffset
Sdummy = Sdummy * Ufaktor
Uzu = Int(sdummy)
Uwsum = 0
'Strom berechnen
Sdummy = Iwsum / Wcount
Sdummy = Sdummy - Ioffset
Sdummy = Sdummy * Ifaktor
Izu = Int(sdummy)
Iwsum = 0
Wcount = 0
'</Messwerte berechnen>
Else 'wird nie erreicht weil weder Hauptsteuerung noch Display diese
'Werte senden
Select Case Empfpuffer(3)
Case 3 'Sinus(1) empfangen
Sinus(1) = Empfpuffer(4)
Writeeeprom Sinus(1) , 17
Case 4 'Sinus(2) empfangen
Sinus(2) = Empfpuffer(4)
Writeeeprom Sinus(2) , 18
Case 5 'Sinus(3) empfangen
Sinus(3) = Empfpuffer(4)
Writeeeprom Sinus(3) , 19
Case 6 'Sinus(4) empfangen
Sinus(4) = Empfpuffer(4)
Writeeeprom Sinus(4) , 20
Case 7 'Sinus(5) empfangen
Sinus(5) = Empfpuffer(4)
Writeeeprom Sinus(5) , 21
Case 8 'Sinus(6) empfangen
Sinus(6) = Empfpuffer(4)
Writeeeprom Sinus(6) , 22
Case 9 'Sinus(7) empfangen
Sinus(7) = Empfpuffer(4)
Writeeeprom Sinus(7) , 23
Case 10 'Sinus(8) empfangen
Sinus(8) = Empfpuffer(4)
Writeeeprom Sinus(8) , 24
Case 11 'Sinus(9) empfangen
Sinus(9) = Empfpuffer(4)
Writeeeprom Sinus(9) , 25
Case 12 'Sinus(10) empfangen
Sinus(10) = Empfpuffer(4)
Writeeeprom Sinus(10) , 26
Case 13 'Sinus(11) empfangen
Sinus(11) = Empfpuffer(4)
Writeeeprom Sinus(11) , 27
Case 14 'Sinus(12) empfangen
Sinus(12) = Empfpuffer(4)
Writeeeprom Sinus(12) , 28
Case 15 'Spannung Kalibrieren (Byte 1)
Kal(1) = Empfpuffer(4)
Case 16 'Spannung Kalibrieren (Byte 2)
Kal(2) = Empfpuffer(4)
Sdummy = Uwsum / Wcount
Ufaktor = Kalibrierwert / Sdummy
Writeeeprom Ufaktor , 5
Uoffset = 0 'Schaltungsbedingt
Writeeeprom Uoffset , 1
Case 17 'Strom Kalibrieren (Byte 1)
Kal(1) = Empfpuffer(4)
Case 18 'Strom Kalibrieren Byte (2)
Kal(2) = Empfpuffer(4)
Sdummy = Iwsum / Wcount
If Kalibrierwert = 0 Then 'Das ist der Offset
Ioffset = Sdummy + 1
Writeeeprom Ioffset , 9
Else 'Jetzt den Faktor berechnen
Sdummy = Sdummy - Ioffset
Ifaktor = Kalibrierwert / Sdummy
Writeeeprom Ifaktor , 13
End If
Case 19
Vwinkel = Empfpuffer(4)
Writeeeprom Vwinkel , 29
Case 20
Rwinkel = Empfpuffer(4)
Writeeeprom Rwinkel , 30
End Select
End If
!LDI R24,0
!sTS {Empfpuffer(1)},R24 'Write Rxx to address loaction
!sTS {Empfpuffer(2)},R24 'Write Rxx to address loaction
!sTS {Empfpuffer(3)},R24 'Write Rxx to address loaction
!sTS {Empfpuffer(4)},R24 'Write Rxx to address loaction
!sTS {Empfpuffer(5)},R24 'Write Rxx to address loaction
End If
Enable Urxc
End If
End If
End If
'</Serielle Telegramme abarbeiten>
'Lüfter einschalten wenn Temperatur > 57 GradC 'Lüfter ausschalten wenn Temperatur < 45 GradC If Temp < 350 Then Fan = 1 Fancount = 120000 'Nachlaufzeit, Zykluszeit 1/100 Sekunde = 150 Sekunden Else If Temp > 532 Then If Fancount = 0 Then 'Nachlaufzeit beendet Fan = 0 End If End If End If If Fancount > 0 Then Decr Fancount
'Notaus mit Freilauf wenn keine Nachricht von Seriell If Loopcount < 50 Then 'Das sind schätzungsweise 0.1s Notaus = 0 Incr Loopcount Else Notaus = 1 Sollspeed = 0 Setspeed End If
Loop
Kommutieren:
!push R10 'Rxx sichern
!PUSH R11 'Rxx sichern
!PUSH R16 'Rxx sichern
!PUSH R17 'Rxx sichern
!PUSH R20 'Rxx sichern
!PUSH R21 'Rxx sichern
!PUSH R24 'Rxx sichern
!PUSH R26 'Rxx sichern
!PUSH R27 'Rxx sichern
!IN R24,sreg
!PUSH R24
!clr R16 'Clear Rxx
#if Numhall = 3
!andi R17,&B01110111 'Clear Hall-D Bits
#else
!lds R17, $0023 'Port B input pins address. Hall D
!BST R17,0 'Store bit x of Rxx in T Flag. Hall D-Bit
!BLD R16,3 'Copy T Flag in the SREG (Status Register) to Bit x in Rxx
#endif
!LDS R17, $0029 'Port D input pins address. Hall A,B,C
!BST R17,2 'Store bit x of Rxx in T Flag. Hall C-Bit
!bld R16,2 'Copy T Flag in the SREG (Status Register) to Bit x in Rxx
!BST R17,4 'Store bit x of Rxx in T Flag. Hall B-Bit
!bld R16,1 'Copy T Flag in the SREG (Status Register) to Bit x in Rxx
!BST R17,7 'Store bit x of Rxx in T Flag. Hall A-Bit
!bld R16,0 'Copy T Flag in the SREG (Status Register) to Bit x in Rxx
'R16 hat nun 0000DCBA Hall Signale
!lds R17,{aposition} 'Load Direct from data space @address xxx
!Swap R17 'R17 hat nun DCBA0000 Hall Signale
!andi R17,&B11110000 'Bit-And of Rxx with a constant value. Die 4 Bit sind Hall A...D
'R17 hat nun nur noch die oberen 4 Bit
!add R17,R16 'Rxx + Ryy. Ergebnis in Rxx
!STS {aposition},R17 'Write Rxx to address loaction
!CLR R17
!LDI R26,$FF 'Startadresse von A() - 1
!LDI R27,$00
!ADD R26,R16
!ADC R27,R17
!LD R24,X 'Load Direct from address xxx
!LDS R16,{Sollrichtung} 'Load Direct from data space @address xxx
!CPI R16,0
!BREQ Sollrichtungnull 'Branch out if equal
!Jmp Sollrichtungnichtnull
Sollrichtungnull:
!LDS R20,{Vwinkel} 'Load Direct from data space @address xxx
!Add R24,R20
!JMP Richtunggesetzt
Sollrichtungnichtnull:
!LDS R20,{Rwinkel}
!Add R24,R20
Richtunggesetzt:
#if Numhall = 3 !CPI R24,$07 'Compare Rxx with constant 6 + 1 Sinusschritte #else !CPI R24,$0D 'Compare Rxx with constant 12 + 1 Sinusschritte #endif !BRCC Istgroesser13_1 !JMP Istkleiner13_1
Istgroesser13_1:
#if Numhall = 3 !SUBI R24,$06 'Subtract constant from Rxx, result in Rxx. 6 Sinusschritte #else !SUBI R24,$0C 'Subtract constant from Rxx, result in Rxx. 12 Sinusschritte #endif
Istkleiner13_1:
!CLR R20
!Ldi R26,$0E 'Startadresse von S() - 1
!LDI R27,$01 'Startadresse von S() - 1
!ADD R26,R24
!ADC R27,R20
!LD R16,X 'Load Direct from address xxx
!STS {Ta},R16 'Write Rxx to address loaction
!subi R24,$fc
#if Numhall = 3
!CPI R24,$07 'Compare Rxx with constant 6 + 1 Sinusschritte
#else
!CPI R24,$0D 'Compare Rxx with constant 12 + 1 Sinusschritte
#endif
!BRCC Istgroesser13_2
!JMP Istkleiner13_2
Istgroesser13_2:
#if Numhall = 3 !SUBI R24,$06 'Subtract constant from Rxx, result in Rxx. 6 Sinusschritte #else !SUBI R24,$0C 'Subtract constant from Rxx, result in Rxx. 12 Sinusschritte #endif
Istkleiner13_2:
!CLR R20
!Ldi R26,$0E 'Startadresse von S() - 1
!LDI R27,$01 'Startadresse von S() - 1
!ADD R26,R24
!ADC R27,R20
!LD R16,X 'Load Direct from address xxx
!STS {Tc},R16 'Write Rxx to address loaction
!subi R24,$fc
#if Numhall = 3
!CPI R24,$07 'Compare Rxx with constant 6 + 1 Sinusschritte
#else
!CPI R24,$0D 'Compare Rxx with constant 12 + 1 Sinusschritte
#endif
!BRCC Istgroesser13_3
!JMP Istkleiner13_3
Istgroesser13_3:
#if Numhall = 3 !SUBI R24,$06 'Subtract constant from Rxx, result in Rxx. 6 Sinusschritte #else !SUBI R24,$0C 'Subtract constant from Rxx, result in Rxx. 12 Sinusschritte #endif
Istkleiner13_3:
!CLR R20
!Ldi R26,$0E 'Startadresse von S() - 1
!LDI R27,$01 'Startadresse von S() - 1
!ADD R26,R24
!ADC R27,R20
!LD R16,X 'Load Direct from address xxx
!STS {Tb},R16 'Write Rxx to address loaction
!lds R10,{aposition}
!CLR R11 'Clear Rxx
!LDI R26,$1A 'Startadresse von Richtung() - 1
!LDI R27,$01 'Startadresse von Richtung() - 1
!ADD R26,R10 'move Ptr by {aposition} innnerhalb Richtung()
!ADC R27,R11 'Add Zero
!LD R16,X+ 'Load Rxx with data from address in X Register(R26, R27) and increment Ptr
'R16 hat Inhalt von Richtung(X+)
!CLR R17 'Clear Rxx
!LDI R20,$02 '2 Byte
!LDI R21,$00
!Sub R16 , R20 'Werte von einander abziehen
!SBC R17,R21 'Werte von einander abziehen
!LDI R26,$1A 'Startadresse von IstRichtung()
!LDI R27,$02 'Startadresse von IstRichtung()
!ST X+,R16 'Store Rxx in address in X + 1 Register
!ST X,R17
!LDI R26,$41 'Startadresse von Ticks()
!LDI R27,$02 'Startadresse von Ticks()
!LD R16,X+ 'Load Rxx with data from address in X Register
!LD R17,X
!LDI R26,$1A 'Startadresse von IstRichtung()
!LDI R27,$02 'Startadresse von IstRichtung()
!LD R20,X+ 'Load Rxx with data from address in X Register
!LD R21,X
!ADD R16,R20
!ADC R17,R21
!LDI R26,$41 'Startadresse von Ticks()
!LDI R27,$02 'Startadresse von Ticks() !ST X+,R16 'Store Rxx in address in X + 1 Register !ST X,R17 !LDS R24,timsk2 !ORI R24,$01 !STS timsk2,R24 'Write Rxx to address loaction !POP R24 !Out Sreg , R24 !POP R27 'Rxx wieder herstellen !POP R26'Rxx wieder herstellen !POP R24'Rxx wieder herstellen !POP R21 'Rxx wieder herstellen !POP R20 'Rxx wieder herstellen !POP R17'Rxx wieder herstellen !POP R16 'Rxx wieder herstellen !POP R11 'Rxx wieder herstellen !POP R10 'Rxx wieder herstellen
Return
Update_pwm: 'Ta, Tb und Tc sind die Amplituden der 3 Phasen
!PUSH R24
!IN R24,sreg
!PUSH R24
!lds R24,{Ta}
!STS ocr2a,R24 'Write Rxx to address loaction
!STS ocr2b,R24
!LDS R24,{Tb}
!STS ocr1al,R24 'Write Rxx to address loaction
!STS ocr1bl,R24
!lds R24,{Tc}
!Out Ocr0a , R24
!Out Ocr0b , R24
!LDS R24,timsk2
!ANDI R24,$FE
!STS timsk2,R24 'Write Rxx to address loaction
!POP R24 'Rxx wieder herstellen
!Out Sreg , R24
!POP R24 'Rxx wieder herstellen
Return
Datenempfang:
'Die empfangenen Zeichen im nach links laufenden Umlauf in Empfpuffer(1)..Empfpuffer(6) geschrieben
'Dadurch wir sichergestellt, dass das Startzeichen (CR) irgendwann in EP0 steht
!PUSH R24 'Rxx sichern
!IN R24, SREG 'Statusregistr sichern
!PUSH R24
'Zeichen im Puffer nach links schieben
!LDS R24,{Empfpuffer(2)} 'Load Direct from data space @address xxx
!STS {Empfpuffer(1)},R24 'Write Rxx to address loaction
!LDS R24,{Empfpuffer(3)} 'Load Direct from data space @address xxx
!STS {Empfpuffer(2)},R24 'Write Rxx to address loaction
!LDS R24,{Empfpuffer(4)} 'Load Direct from data space @address xxx
!STS {Empfpuffer(3)},R24 'Write Rxx to address loaction
!LDS R24,{Empfpuffer(5)} 'Load Direct from data space @address xxx
!STS {Empfpuffer(4)},R24 'Write Rxx to address loaction
!LDS R24,{Empfpuffer(6)} 'Load Direct from data space @address xxx
!STS {Empfpuffer(5)},R24 'Write Rxx to address loaction
'neues Zeichen einlesen
!IN R24,UDR 'Load an I/O Location to Rxx
!STs {Empfpuffer(6)},R24 'Write Rxx to address loaction
!POP R24 'Rxx wieder herstellen
!Out Sreg , R24 'Statusregister wieder herstellen
!POP R24 'Rxx wieder herstellen
Return
Sub Setspeed
If Notaus = 1 Then
Pwm_stop
Sollspeed = 0
For N = 1 To 12
S(n) = 0
Next
Else
#if Numhall = 3 'Anzahl Hallsensoren
For N = 1 To 6
Idummy1 = Sollspeed * Sinus(n)
Idummy1 = Idummy1 / 100
S(n) = Idummy1
Next
#else
For N = 1 To 12
Idummy1 = Sollspeed * Sinus(n)
Idummy1 = Idummy1 / 100
S(n) = Idummy1
Next
#endif
If Tccr0a = 0 Then Pwm_start 'Wenn die PWM aus ist
'<Zwangskommutieren??> Zwangskommutieren = 0 If Sollrichtung <> Altsollrichtung Then Zwangskommutieren = 1 If Sollspeed <> Altsollspeed Then Zwangskommutieren = 1
If Zwangskommutieren = 1 Then
Pcicr = &B00000000 'Den eigentlichen Kommutierinterrupt abschalten
!clr R16 'Clear Register 16
!lds R17, $0023 'Load Direct from data space @address xxx
!BST R17,0 'Store bit x of Rxx in T Flag
!BLD R16,3 'Copy T Flag in the SREG (Status Register) to Bit x in Rxx
!LDS R17, $0029 'Load Direct from data space @address xxx
!BST R17,2 'Store bit x of Rxx in T Flag
!bld R16,2 'Copy T Flag in the SREG (Status Register) to Bit x in Rxx
!BST R17,4 'Store bit x of Rxx in T Flag
!bld R16,1 'Copy T Flag in the SREG (Status Register) to Bit x in Rxx
!BST R17,7 'Store bit x of Rxx in T Flag
!bld R16,0 'Copy T Flag in the SREG (Status Register) to Bit x in Rxx
!lds R17,{aposition} 'Load Direct from data space @address xxx
!Swap R17 'Swap high and low nibbles in Rxx. aposition is now in the upper nibble
!andi R17,&B11110000 'Logical AND Rxx and a constant, Result in Rxx. Clear lower nibble
!add R17,R16 'Add Rxx + Ryy, Result in Rxx
!STS {aposition},R17 'Write Rxx to address loaction
!CLR R17 'Clear Rxx
!LDI R26,$FF 'Startadresse von A() - 1
!LDI R27,$00 'Startadresse von A() - 1
!ADD R26,R16 'Add Rxx + Ryy without carry, Result in Rxx
!ADC R27,R17 'Add Rxx + Ryy with carry, Result in Rxx
!LD R24,X 'Load Indirect Rxx with address in X Register
!LDS R16,{Sollrichtung} 'Load Direct from data space @address xxx
!CPI R16,0 'Compare Rxx with constant
!BREQ Sollrichtungnullz 'Branch if Equal
!Jmp Sollrichtungnichtnullz 'Relative Jump
Sollrichtungnullz:
!LDS R20,{Vwinkel} 'Load Direct from data space @address xxx
!Add R24,R20 'Add Rxx + Ryy, Result in Rxx
!JMP Richtunggesetztz
Sollrichtungnichtnullz:
!LDS R20,{Rwinkel} 'Load Direct from data space @address xxx
!Add R24,R20 'Add Rxx + Ryy without carry, Result in Rxx
Richtunggesetztz:
#if Numhall = 3
!CPI R24,$07 'Compare Rxx with constant 6 + 1 Sinusschritte
#else
!CPI R24,$0D 'Compare Rxx with constant 12 + 1 Sinusschritte
#endif
!BRCC Istgroesser13_1z 'Branch if Carry Cleared
!JMP Istkleiner13_1z 'Relative Jump
Istgroesser13_1z:
#if Numhall = 3
!SUBI R24,$06 'Subtract constant from Rxx, result in Rxx. 6 Sinusschritte
#else
!SUBI R24,$0C 'Subtract constant from Rxx, result in Rxx. 12 Sinusschritte
#endif
Istkleiner13_1z:
!CLR R20 'Clear Rxx
!Ldi R26,$0E 'Startadresse von S() - 1
!LDI R27,$01 'Startadresse von S() - 1
!ADD R26,R24 'Add Rxx + Ryy without carry, Result in Rxx
!ADC R27,R20 'Add Rxx + Ryy with carry, Result in Rxx
!LD R16,X 'Load Rxx with data from address in X Register
!STS {Ta},R16 'Write Rxx to address loaction
!subi R24,$fc 'Subtract constant from Rxx, result in Rxx
#if Numhall = 3
!CPI R24,$07 'Compare Rxx with constant 6 + 1 Sinusschritte
#else
!CPI R24,$0D 'Compare Rxx with constant 12 + 1 Sinusschritte
#endif
!BRCC Istgroesser13_2z 'Branch if Carry Cleared
!JMP Istkleiner13_2z 'Relative Jump
Istgroesser13_2z:
#if Numhall = 3
!SUBI R24,$06 'Subtract constant from Rxx, result in Rxx. 6 Sinusschritte
#else
!SUBI R24,$0C 'Subtract constant from Rxx, result in Rxx. 12 Sinusschritte
#endif
Istkleiner13_2z:
!CLR R20 'Clear Rxx
!Ldi R26,$1a 'Startadresse von S() - 1
!LDI R27,$01 'Startadresse von S() - 1
!ADD R26,R24 'Add Rxx + Ryy without carry, Result in Rxx
!ADC R27,R20 'Add Rxx + Ryy with carry, Result in Rxx
!LD R16,X 'Load Rxx with data from address in X Register
!STS {Tc},R16 'Write Rxx to address loaction
!subi R24,$fc 'Subtract constant from Rxx, result in Rxx
#if Numhall = 3
!CPI R24,$07 'Compare Rxx with constant 6 + 1 Sinusschritte
#else
!CPI R24,$0D 'Compare Rxx with constant 12 + 1 Sinusschritte
#endif
!BRCC Istgroesser13_3z 'Branch if Carry Cleared
!JMP Istkleiner13_3z 'Relative Jump
Istgroesser13_3z:
#if Numhall = 3
!SUBI R24,$06 'Subtract constant from Rxx, result in Rxx. 6 Sinusschritte
#else
!SUBI R24,$0C 'Subtract constant from Rxx, result in Rxx. 12 Sinusschritte
#endif
Istkleiner13_3z:
!CLR R20 'Clear Rxx
!Ldi R26,$1a 'Startadresse von S() - 1
!LDI R27,$01 'Startadresse von S() - 1
!ADD R26,R24 'Add Rxx + Ryy without carry , Result in Rxx
!ADC R27,R20 'Add Rxx + Ryy with carry, Result in Rxx
!LD R16,X 'Load Rxx with data from address in X Register
!STS {Tb},R16 'Write Rxx to address loaction
Pcicr = _pcicr 'Den eigentlichen Kommutierinterrupt einschalten
Timsk2.toie2 = 1 'Timer2 Overflow Interrupt aktivieren
Toggle Led2
End If
'</Zwangskommutieren>
End If
Altsollrichtung = Sollrichtung
Altsollspeed = Sollspeed
End Sub
Sub Pwm_start Disable Interrupts 'Damit die Timer auch wirklich synchron laufen
Tccr0a = 161 'Timer Counter Control Register A Tccr0b = 1 'Timer Counter Control Register B Ocr0a = 0 'Set output compare Ctr 0, Register A, continuously compared with TCNT0 Ocr0b = 0 'Set output compare Ctr 0, Register B, continuously compared with TCNT0 Tccr1a = 161 'Timer Counter Control Register 1A Tccr1b = 1 'Timer Counter Control Register 1B Ocr1ah = 0 'Output A compare value in H and L Bytes Ocr1al = 0 Ocr1bh = 0 'Output B compare value in H and L Bytes Ocr1bl = 0 Tccr2a = 161 'Timer Counter Control Register 2A Tccr2b = 1 'Timer Counter Control Register 2B Ocr2a = 0 'Set output compare Ctr 2, Register A, continuously compared with TCNT0 Ocr2b = 0 'Set output compare Ctr 2, Register B, continuously compared with TCNT0 Tcnt0 = 0 'Timer Startwert Tcnt1 = 6 'Timer Startwert, Wert anders damit sie alle synchron laufen Tcnt2 = 9 'Timer Startwert, Wert anders damit sie alle synchron laufen
Enable Interrupts End Sub
Sub Pwm_stop
Tccr0a = 0 'Timer Counter Control Register A Tccr0b = 0 'Timer Counter Control Register B Ocr0a = 0 'Set output compare Ctr 0, Register A, continuously compared with TCNT0 Ocr0b = 0 'Set output compare Ctr 0, Register B, continuously compared with TCNT0 Tccr1a = 0 'Timer Counter Control Register 1A Tccr1b = 0 'Timer Counter Control Register 1B Ocr1ah = 0 'Output A compare value in H and L Bytes Ocr1al = 0 Ocr1bh = 0 'Output B compare value in H and L Bytes Ocr1bl = 0 Tccr2a = 0 'Timer Counter Control Register 2A Tccr2b = 0 'Timer Counter Control Register 2B Ocr2a = 0 'Set output compare Ctr 2, Register A, continuously compared with TCNT0 Ocr2b = 0 'Set output compare Ctr 2, Register B, continuously compared with TCNT0 'alle Phasen Aus Portd.3 = 0 'A+ Portb.3 = 1 'A- Portb.2 = 0 'B+ Portb.1 = 1 'B- Portd.5 = 0 'C+ Portd.6 = 1 'C-
End Sub
Hauptsteuerung: '( ******************Listrik01 Hauptsteuerung, Version 01 ******************
1. LED1 blinkt mit 1 Hz wenn alles OK
2. LED1 und LED2 blinken mit 5Hz wenn I2c-Bus gestört ist
3. LED1 und LED2 blinken mit bis zu 10Hz wenn U-Batt zu klein ist. Frequenz hängt
von der Anzahl Interrups ab, die während der Wartezeit auflaufen, z.B. von COM
4. Lade-LEDs
- blinken mit 4Hz wenn Gyro nicht auf Null ist
- blinken mit 5Hz wenn Reset button gedrückt wird und Gyrosignale groß sind
- machen Lichtorgel wenn Kopplung zum Motorcontroler gestört ist. Siehe Punkt 6
5. Gyro wird nur zur Initialisierung benutzt. Fahrzeug muss still stehen beim START
6. Kommunikation zum Motorcontroller ist gestört (Timeout) wenn der Hallstecker
am Motorcontroller nicht steckt sind! Dadurch antwortet der Motorcontroller dem Hauptcontroller nicht. Er sendet z.B. keine Spannungsinfo an den Hauptcontroller. Dadurch macht Dieser ständig einen Restart. Viele Werte werden dann nicht zum Dashboard übertragen!
LockBit, FuseBit , FuseBitHigh , FuseBitExtended $prog &HFF , &HE7 , &HD9 , &HFF 'Fusebits für Atmega644PA 20MHz
Use AMTEL Studio / Fuses Menu instead to set fusebits BODLEVEL = disabled OCDEN = Disabled JTAGEN = Disabled SPIEN = Checked WDTON = Disabled EESAVE = Disabled BOOTSZ = 4096W_7000 BOOTRST = Disabled CKDIV8 = Disabled CKOUT = Disabled SUT_CKSEL = FSOSZ_16KCK_4MS1_XOSC_FASTPWR ')
$regfile = "m644def.dat" 'ATMEGA644PA CPU $lib "i2c_twi.lbx" $framesize = 64 $swstack = 64 $hwstack = 64 $crystal = 20000000 $baud = 57600 'No parity, 1 stop bit, 8 data bits
Declare Sub Onresetbtn Declare Sub Logtext(byval Adresse As Word , Text As String ) Declare Sub Akkugrenzen Declare Sub Unfalldatenschreiber Declare Sub Betriebsdatenschreiber Declare Sub Flashled(byval Which As Byte , Byval Dauer_ms As Byte) Declare Sub Resetdata
' Porta.0 = ADC ' Porta.1 = ADC Config Porta.2 = Input 'not used Config Porta.3 = Input 'not used Config Porta.4 = Input 'not used Config Porta.5 = Input 'not used Config Porta.6 = Input 'not used Config Porta.7 = Input 'not used 'Pull up Widerstände der unbenutzten Eingänge einschalten Porta = 1
Config Portb.0 = Input 'not used Config Portb.1 = Output 'Gyro Self Test Config Portb.2 = Output 'Gyro Reset Config Portb.3 = Output 'Led 2 Config Portb.4 = Output 'LED 1 ' PORTB.5 'MOSI ' PORTB.6 'MISO ' PORTB.7 'SCK
Config Scl = Portc.0 Config Sda = Portc.1
Config Portc.2 = Input 'Fussschalter 1 Config Portc.3 = Input 'Fussschalter 2 Config Portc.4 = Input 'not used Config Portc.5 = Input 'not used Config Portc.6 = Input 'not used Config Portc.7 = Output 'Beeper 'Pull up Widerstände der Eingänge Ein/Ausschalten Portc.2 = 0 Portc.3 = 0 Portc.4 = 1 Portc.5 = 1 Portc.6 = 1
' PORTD.0 = 'RXD ' PORTD.1 = 'TXD Config Portd.2 = Input 'Reset Button vom Akku-Display Config Portd.3 = Output 'Batterieanzeige Config Portd.4 = Output 'Batterieanzeige Config Portd.5 = Output 'Batterieanzeige Config Portd.6 = Output 'Batterieanzeige Config Portd.7 = Output 'Batterieanzeige 'Pull up Widerstände einschalten Portd.2 = 1
Led1 Alias Portb.4 Led2 Alias Portb.3 Led3 Alias Portd.4 'Batterieladezustandsanzeige 0..20% Led4 Alias Portd.5 'Batterieladezustandsanzeige 21..40% Led5 Alias Portd.6 'Batterieladezustandsanzeige 41..60% Led6 Alias Portd.3 'Batterieladezustandsanzeige 61..80% Led7 Alias Portd.7 'Batterieladezustandsanzeige 81..100%
Beeper Alias Portc.7 Resetbtn Alias Pind.2
'Sensortest gibt es bei I2C Gyro nicht 'Sensor_aus Alias Portb.2 '1=Aus 0=Ein 'Sensor_test Alias Portb.1 '1=Self-Test. 0=Normalbetrieb
Config Debounce = 100 '[mS] when the config statement is not used a ' default of 25mS will be used '******************************Gyro über I2C ******************************** '##### 7 - Bit Adresses Gyro ######## Const Who_am_i = &H0F 'auch V2 Const Ctrl_reg1 = &H20 'auch V2 Const Ctrl_reg2 = &H21 'auch V2 Const Ctrl_reg3 = &H22 'auch V2 Const Ctrl_reg4 = &H23 'auch V2 Const Ctrl_reg5 = &H24 'auch V2 Const Out_temp = &H26 'auch V2 Const Status_reg = &H27 'auch V2 Const Out_x_l = &H28 'auch V2 Const Out_x_h = &H29 'auch V2 Const Out_y_l = &H2A 'auch V2 Const Out_y_h = &H2B 'auch V2 Const Out_z_l = &H2C 'auch V2 Const Out_z_h = &H2D 'auch V2 '##### 7-Bit Adresses Accelerometer ######## Const Ctrl_reg1_a = &H20 'auch V2 Const Ctrl_reg2_a = &H21 'auch V2 Const Ctrl_reg3_a = &H22 'auch V2 Const Ctrl_reg4_a = &H23 'auch V2 Const Ctrl_reg5_a = &H24 'auch V2 Const Status_reg_a = &H27 'auch V2 Const Out_x_l_a = &H28 'auch V2 Const Out_x_h_a = &H29 'auch V2 Const Out_y_l_a = &H2A 'auch V2 Const Out_y_h_a = &H2B 'auch V2 Const Out_z_l_a = &H2C 'auch V2 Const Out_z_h_a = &H2D 'auch V2 '##### 7-Bit Adresses Magnetometer ######## 'Const Cra_reg_m = &H00 'auch V2 'Const Crb_reg_m = &H01 'auch V2 'Const Mr_reg_m = &H02 'auch V2 'Const Out_x_h_m = &H03 'auch V2
'Immer *2 weil Adresse bei Bit1 beginnt. Bit 0 ist für Read/Write Const Gyroadress_w = 210 '=105*2, =107d*2 bei V2. Gyro slave adress for write Const Gyroadress_r = 211 '=(105*2) + 1, =107d*2 bei V2. Gyro slave adress for write Const Accadress_w = 48 '=24*2, auch bei V2 Accelerometer slave adress for write Const Accadress_r = 49 '=(24*2) + 1, auch bei V2 Accelerometer slave adress for write
'Const Magadress_w = 60 '=30*2, auch bei V2 Magnetometer slave adress for write 'Const Magadress_r = 61 '=(30*2) + 1, auch bei V2 Magnetometer slave adress for write '--------------------------------------------------------------------
'>>>>>>>>>>>>>>>> DEFAULT WERTE <<<<<<<<<<<<<<<<<<
Const Fussschalterschwelle = 750 'ADC Wert
'Konstanten für die Geschwindigkeits und Streckenmessung Const Kom_pro_u = 276 '276*Kommutieren für eine Umdrehung. Const Radumpfang = 139 'RaduMPFang in Zentimeter 'Maximalwert bis zu dem die Abweichung in y aufaddiert wird Const Max_mit_acc_nick_s = 1024 Const Min_mit_acc_nick_s = -1024 Const Max_lenk = 128 Const Min_lenk = -128 'Konstanten für die Leistungsbegrenzung, Geschwindigkeitsbegrenzung 'Maximalleistung Const Maxpwm = 255 Const Minpwm = -255 'PWM ab der automatisch gebremst wird um blos nie an die Grenze zu kommen Const Spwm = 128 Const Spwmr = -128 'Offsetwerte für die Mittellage Const Offset_roll = 67700 Const Offset_nick = 65400 Const O_roll_u = 66200 Const O_roll_o = 69200 Const O_nick_u = 63900 Const O_nick_o = 66900 Const O_nick_oo = 67700
Const Vmax = 120 '12kmh Vmax Vorwärts in km/h*10 Const Vmaxr = -120 '-12Kmh Vmax Rückwärts in km/h*10
Const Systemvoltage = 14 '14, 24, 36, 48V
'Konstanten für die Akkus
- if Systemvoltage = 14 'Nur zum Test mit Netzteil
Const Akkuleer = 1300 Const Akkufastleer = 1330
- endif
- if Systemvoltage = 24
'24V, 12V pro Akku * 2 Akku Const Akkuleer = 2400 Const Akkufastleer = 2500
- endif
- if Systemvoltage = 36
'36V, 12V pro Akku * 3 Akku Const Akkuleer = 3600 Const Akkufastleer = 3750
- endif
- if Systemvoltage = 48
'48V, 12V pro Akku * 4 Akku Const Akkuleer = 4800 Const Akkufastleer = 4950
- endif
'Konstanten für die Signalaufbereitung Const Gyro_anz = 10 'war 10000 Const Acc_anz = 5 'Anzahl der Messungen zur Mittelwertbildung Const Gyro_faktor = 1 'war 91 '****************************************************************************
Dim Ugemessen As Long 'Batteriespannung in V*100, kommt vom Motorcontr. Dim Igemessen As Long 'Gesamter Motorstrom in mA, kommt vom Motorcontr.
Dim U As Integer Dim I As Integer At U + 2 Dim K As Integer At U + 4 Dim Checksumme As Byte At U + 6 Dim Ende As Byte At U + 7 Dim Motormeldung(8) As Byte At U + 0 Overlay
Dim M1 As Byte At U + 0 Overlay Dim M2 As Byte At U + 1 Overlay Dim M3 As Byte At I + 0 Overlay Dim M4 As Byte At I + 1 Overlay Dim M5 As Byte At K + 0 Overlay Dim M6 As Byte At K + 1 Overlay Dim M7 As Byte At Checksumme Overlay Dim M8 As Byte At Ende Overlay
'Variablen für die Telegramme an die Motorregler Dim Checksum_rechts As Byte Dim Checksum_links As Byte Dim Richtung_rechts As Byte Dim Richtung_links As Byte Dim Pwm_rechts As Byte Dim Pwm_links As Byte Dim Zeiger_meldung As Byte Dim Ta As Byte 'Interruptdauer. Wenn lang, dann ist der Zählwert nahe 255 Dim U_links As Integer 'Motorspannung linker Motor Dim U_rechts As Integer 'Motorspannung rechter Motor Dim I_links As Integer 'Motorstrom linker Motor Dim I_rechts As Integer 'Motorstrom rechter Motor Dim K_links As Integer 'Anzahl Kommutierungen linker Motor Dim K_rechts As Integer 'Anzahl Kommutierungen rechter Motor Dim B(5) As Byte
Dim Kilometer As Word Dim Kilometermeldung(2) As Byte At Kilometer Overlay
Dim Akkustand As Integer Dim Akkumeldung(2) As Byte At Akkustand Overlay
Dim Kmh As Integer Dim Kmhmeldung(2) As Byte At Kmh Overlay
Dim Meter As Integer Dim Metermeldung(2) As Byte At Meter Overlay
Dim Di As Single 'Distanz pro Kommutieren Dim Kmhfaktor As Single Dim Zentimeter As Single Dim Strecke As Single 'Gefahrene Strecke während 1/100s Dim Kx As Single 'Anzahl Kommutierungen für Streckenberechnung Dim Kv As Single 'Anzahl Kommutierungen für Geschw.Berechnung Dim Vbrems As Long 'Bremse bei Überlast oder zu hoher Geschw. Dim Lbrems As Long Dim Brems As Long
Dim P_faktor As Single Dim Pf1 As Byte At P_faktor + 0 Overlay Dim Pf2 As Byte At P_faktor + 1 Overlay Dim Pf3 As Byte At P_faktor + 2 Overlay Dim Pf4 As Byte At P_faktor + 3 Overlay
Dim I_faktor As Single Dim If1 As Byte At I_faktor + 0 Overlay Dim If2 As Byte At I_faktor + 1 Overlay Dim If3 As Byte At I_faktor + 2 Overlay Dim If4 As Byte At I_faktor + 3 Overlay
Dim D_faktor As Single Dim Df1 As Byte At D_faktor + 0 Overlay Dim Df2 As Byte At D_faktor + 1 Overlay Dim Df3 As Byte At D_faktor + 2 Overlay Dim Df4 As Byte At D_faktor + 3 Overlay
Dim Gyro_nick As Integer 'war word Dim Gyro_nick1 As Byte At Gyro_nick + 0 Overlay Dim Gyro_nick2 As Byte At Gyro_nick + 1 Overlay
Dim Gyro_roll As Integer 'war word Dim Gyro_roll1 As Byte At Gyro_roll + 0 Overlay Dim Gyro_roll2 As Byte At Gyro_roll + 1 Overlay
Dim Gyro_gier As Integer 'war word Dim Gyro_gier1 As Byte At Gyro_gier + 0 Overlay Dim Gyro_gier2 As Byte At Gyro_gier + 1 Overlay
Dim Gyro_roll_alt As Integer 'war word Dim Gyro_nick_alt As Integer 'war word
Dim Acc_nick As Long Dim Acc_nick1 As Byte At Acc_nick + 0 Overlay Dim Acc_nick2 As Byte At Acc_nick + 1 Overlay
Dim Accnick As Integer Dim Accnick_l As Byte At Accnick + 0 Overlay Dim Accnick_h As Byte At Accnick + 1 Overlay
Dim Acc_roll As Long Dim Acc_roll1 As Byte At Acc_roll + 0 Overlay Dim Acc_roll2 As Byte At Acc_roll + 1 Overlay
Dim Accroll As Integer Dim Accroll_l As Byte At Accroll + 0 Overlay Dim Accroll_h As Byte At Accroll + 1 Overlay
Dim Acc_gier As Long Dim Acc_gier1 As Byte At Acc_gier + 0 Overlay Dim Acc_gier2 As Byte At Acc_gier + 1 Overlay
Dim Accgier As Integer Dim Accgier_l As Byte At Accgier + 0 Overlay Dim Accgier_h As Byte At Accgier + 1 Overlay
Dim Sum_acc_roll As Long Dim Sum_acc_nick As Long Dim Sum_gyro_roll As Long Dim Sum_gyro_nick As Long Dim Korr_acc_roll As Long Dim Korr_acc_nick As Long
'Long ==> Long Integer Dim Mit_acc_nick_s As Long 'Summierter Neigung vorwärts Mittelwert Dim Mit_gyro_roll As Long Dim Mit_gyro_nick As Long Dim Sig_gyro_roll As Long Dim Sig_gyro_nick As Long
Dim Stellwert As Single Dim Stellwert1 As Byte At Stellwert + 0 Overlay Dim Stellwert2 As Byte At Stellwert + 1 Overlay Dim Stellwert3 As Byte At Stellwert + 2 Overlay Dim Stellwert4 As Byte At Stellwert + 3 Overlay
Dim Mrg As Single Dim Mrg1 As Byte At Mrg + 0 Overlay Dim Mrg2 As Byte At Mrg + 1 Overlay Dim Mrg3 As Byte At Mrg + 2 Overlay Dim Mrg4 As Byte At Mrg + 3 Overlay
Dim Mra As Single Dim Mra1 As Byte At Mra + 0 Overlay Dim Mra2 As Byte At Mra + 1 Overlay Dim Mra3 As Byte At Mra + 2 Overlay Dim Mra4 As Byte At Mra + 3 Overlay
Dim Kksum As Single Dim Kksum1 As Byte At Kksum + 0 Overlay Dim Kksum2 As Byte At Kksum + 1 Overlay Dim Kksum3 As Byte At Kksum + 2 Overlay Dim Kksum4 As Byte At Kksum + 3 Overlay
Dim Gyronick As Long Dim Gyronick1 As Byte At Gyronick + 0 Overlay Dim Gyronick2 As Byte At Gyronick + 1 Overlay Dim Gyronick3 As Byte At Gyronick + 2 Overlay Dim Gyronick4 As Byte At Gyronick + 3 Overlay
Dim Mp_faktor As Single Dim Mpf1 As Byte At Mp_faktor + 0 Overlay Dim Mpf2 As Byte At Mp_faktor + 1 Overlay Dim Mpf3 As Byte At Mp_faktor + 2 Overlay Dim Mpf4 As Byte At Mp_faktor + 3 Overlay
Dim Mv_faktor As Single Dim Mvf1 As Byte At Mv_faktor + 0 Overlay Dim Mvf2 As Byte At Mv_faktor + 1 Overlay Dim Mvf3 As Byte At Mv_faktor + 2 Overlay Dim Mvf4 As Byte At Mv_faktor + 3 Overlay
Dim Mpl_faktor As Single Dim Mplf1 As Byte At Mpl_faktor + 0 Overlay Dim Mplf2 As Byte At Mpl_faktor + 1 Overlay Dim Mplf3 As Byte At Mpl_faktor + 2 Overlay Dim Mplf4 As Byte At Mpl_faktor + 3 Overlay
Dim Mvl_faktor As Single Dim Mvlf1 As Byte At Mvl_faktor + 0 Overlay Dim Mvlf2 As Byte At Mvl_faktor + 1 Overlay Dim Mvlf3 As Byte At Mvl_faktor + 2 Overlay Dim Mvlf4 As Byte At Mvl_faktor + 3 Overlay
Dim P_tacho As Single Dim Tachomeldung(30) As Byte At P_tacho Overlay Dim T1 As Byte At P_tacho + 0 Overlay Dim T2 As Byte At P_tacho + 1 Overlay Dim T3 As Byte At P_tacho + 2 Overlay Dim T4 As Byte At P_tacho + 3 Overlay
Dim T5 As Byte At P_tacho + 4 Overlay Dim T6 As Byte At P_tacho + 5 Overlay Dim T7 As Byte At P_tacho + 6 Overlay Dim T8 As Byte At P_tacho + 7 Overlay Dim I_tacho As Single At P_tacho + 4 Overlay
Dim T9 As Byte At P_tacho + 8 Overlay Dim T10 As Byte At P_tacho + 9 Overlay Dim T11 As Byte At P_tacho + 10 Overlay Dim T12 As Byte At P_tacho + 11 Overlay Dim D_tacho As Single At P_tacho + 8 Overlay
Dim T13 As Byte At P_tacho + 12 Overlay Dim T14 As Byte At P_tacho + 13 Overlay Dim T15 As Byte At P_tacho + 14 Overlay Dim T16 As Byte At P_tacho + 15 Overlay Dim Mp_tacho As Single At P_tacho + 12 Overlay
Dim T17 As Byte At P_tacho + 16 Overlay Dim T18 As Byte At P_tacho + 17 Overlay Dim T19 As Byte At P_tacho + 18 Overlay Dim T20 As Byte At P_tacho + 19 Overlay Dim Mv_tacho As Single At P_tacho + 16 Overlay
Dim T21 As Byte At P_tacho + 20 Overlay Dim T22 As Byte At P_tacho + 21 Overlay Dim T23 As Byte At P_tacho + 22 Overlay Dim T24 As Byte At P_tacho + 23 Overlay Dim Mpl_tacho As Single At P_tacho + 20 Overlay
Dim T25 As Byte At P_tacho + 24 Overlay Dim T26 As Byte At P_tacho + 25 Overlay Dim T27 As Byte At P_tacho + 26 Overlay Dim T28 As Byte At P_tacho + 27 Overlay Dim Mvl_tacho As Single At P_tacho + 24 Overlay
Dim T29 As Byte At P_tacho + 28 Overlay Dim T30 As Byte At P_tacho + 29 Overlay Dim Tdummy As Integer At P_tacho + 28 Overlay
Dim Wwert As Word Dim Wwert1 As Byte At Wwert + 0 Overlay Dim Wwert2 As Byte At Wwert + 1 Overlay
Dim Gyro_gier_alt As Integer 'war word Dim Sum_gyro_gier As Long Dim Mit_gyro_gier As Long Dim Sig_gyro_gier As Long
Dim Werteneu As Byte
Dim Mit_acc_roll As Long 'Neigungsmittelwert seitwärts Dim Mit_acc_roll1 As Byte At Mit_acc_roll + 0 Overlay Dim Mit_acc_roll2 As Byte At Mit_acc_roll + 1 Overlay Dim Mit_acc_roll3 As Byte At Mit_acc_roll + 2 Overlay Dim Mit_acc_roll4 As Byte At Mit_acc_roll + 3 Overlay
Dim Mit_acc_nick As Long 'Neigungsmittelwert vorwärts Dim Mit_acc_nick1 As Byte At Mit_acc_nick + 0 Overlay Dim Mit_acc_nick2 As Byte At Mit_acc_nick + 1 Overlay Dim Mit_acc_nick3 As Byte At Mit_acc_nick + 2 Overlay Dim Mit_acc_nick4 As Byte At Mit_acc_nick + 3 Overlay
Dim Null_roll As Long Dim Null_roll1 As Byte At Null_roll + 0 Overlay Dim Null_roll2 As Byte At Null_roll + 1 Overlay Dim Null_roll3 As Byte At Null_roll + 2 Overlay Dim Null_roll4 As Byte At Null_roll + 3 Overlay
Dim Null_nick As Long Dim Null_nick1 As Byte At Null_nick + 0 Overlay Dim Null_nick2 As Byte At Null_nick + 1 Overlay Dim Null_nick3 As Byte At Null_nick + 2 Overlay Dim Null_nick4 As Byte At Null_nick + 3 Overlay
Dim Mit_regelzeit_ges As Single
Dim Lagefehler As Long 'Die Zeit in 1/100s die benötigt wird bis die Nullage erreicht ist Dim Regelzeit As Word Dim Regelzeitmax As Word Dim Mit_acc_roll_max As Long Dim Mit_acc_nick_max As Long Dim Mit_acc_roll_min As Long Dim Mit_acc_nick_min As Long
Dim Pwm_gesamt As Single Dim Pwm_rechts_max As Byte Dim Pwm_links_max As Byte Dim Intcount As Byte Dim K_diff As Single Dim K_sum As Single Dim K_diff_min As Single Dim K_diff_max As Single Dim Pwm_lenk_rechts As Integer Dim Pwm_lenk_links As Integer
Dim Lenkvorgabe As Single Dim Mp_anpassung As Single Dim Mv_anpassung As Single Dim Mpl_anpassung As Single Dim Mvl_anpassung As Single Dim K_sum_anpassung As Single
Dim Wdummy1 As Word Dim Ldummyi As Long Dim Ldummy1 As Long Dim Ldummy2 As Long Dim Bdummy1 As Byte Dim Idummy1 As Integer Dim Sdummy1 As Single
Dim Antriebaus As Byte Dim Notaus As Byte Dim Sensor_ok As Byte 'Zeigt an ob die Mittelwerte der Gyros stabil stehen Dim Sensor_ok_count As Byte Dim Ltest As Byte
Dim Imin As Long 'Kleinster gemessener Motorstrom (größte Rückspeisung) Dim Imax As Long 'größter gemessener Motorstrom Dim Summe_as_entladen As Long Dim Summe_mah_entladen As Long Dim Summe_as_laden As Long Dim Summe_mah_laden As Long Dim Akkukap As Long 'Akkukapazität 0...100 % Dim Akku1 As Long '80% Grenze Dim Akku2 As Long '60% Grenze Dim Akku3 As Long '40% Grenze Dim Akku4 As Long '20% Grenze Dim Akku5 As Long '0% Grenze Dim Erzeugt As Long 'Erzeugte Ladung in mA-Stunden Dim Verbraucht As Long 'Verbrauchte Ladung in mA-Stunden
'Single Variablen um die Prozente rechnen zu können Dim Akkuinhalt As Single 'wie AkkuKap Dim Akkuverbrauch As Single Dim Akkuprozent As Single
Dim Bzeit As Long 'Beruhigungszeit von Gyro und Accelerometer Dim Bstate As Byte 'Status der Beruhigungszeit von Gyro und Accelerometer Dim Frei As Byte 'hat es etwas mit Gyro und Acc zu tun? Dim Timeout As Byte Dim S_err As Byte 'Serielle Schnittstellen Error Dim Seriell_err_links As Word 'COM error links Dim Seriell_err_rechts As Word 'COM error rechts Dim Tamax As Byte 'Maximalwert von Ta Dim Fusscount As Word 'Fusschalter nicht betätigt ==> zeitverzögerung Dim Blinkcount As Byte 'verschiedene Blinkraten Dim Lenkerstellung As Single Dim Lenkstellwert As Single Dim Lenkfehler As Single
'Texte für den Unfalldatenschreiber (als Text im eeprom) Dim Utext As String * 32
Dim Ugrund As String * 32 'Auslösegrund Dim Text As String * 16 'Allg. Ttext Dim Min_v As Integer 'Minimalwert der Geschwindigkeit Dim Max_v As Integer 'Maximalwert der Geschwindigkeit
Dim Tachocount As Byte 'Schleifenzähler um Werte an Display zu schicken Dim Fussschalter As Byte 'Die beiden Fussschalter Dim Adc0 As Word 'Analogwert Fussschalter 1 Dim Adc1 As Word 'Analogwert Fussschalter 2
Dim Slaveadress As Byte 'Adress of the I2C slave to read or write Dim Subadress As Byte 'the I2C register to read or write Dim Databyte As Byte 'I2C Byte to read or write
Dim Resetbtndone As Byte
Dim Nummotors As Byte '1 oder 2 Dim Errorword As Word 'See Delphi Dashboard.prj Dim Errorword1 As Byte At Errorword + 0 Overlay Dim Errorword2 As Byte At Errorword + 1 Overlay
Led1 = 0 Led2 = 0
Led3 = 1 Led4 = 1 Led5 = 1 Led6 = 1 Led7 = 1
'***************** Check! ********************* Nummotors = 1 '***************** Check! *********************
'Ein paar feste Werte vorab berechnen: Di = Radumpfang / Kom_pro_u 'Eigentlich 3.6 aber bei der Mittelwertbildung entsteht der 4-Fache Wert, 'darum 9, und es ergibt sich automateisch V in 0.1km/h 'Unbenutzte Pins auf Masse legen Kmhfaktor = Di * 9
'Sensorboard Schalten 'Sensor_aus = 0 'Sensor_test = 0
Config Adc = Single , Prescaler = Auto , Reference = Internal_2.56 'Avcc 'Start Adc not required because ADC will start immediately after Config ADC
'Inititialise I2C-Bus I2cinit Config Twi = 200000 ' wanted clock frequency in Hertz 'will set TWBR and TWSR 'Twbr = 12 'bit rate register 'Twsr = 0 'pre scaler bits
'TWI gleich einschalten, das macht Bascom ansonsten erst beim I2CStart ! Twcr = &B00000100
Gosub Init_gyro Gosub Init_acc
'Wartezeit bis die Motorregler soweit sind Readeeprom Max_v , $2f0 Readeeprom Min_v , $2f2 Readeeprom Summe_as_entladen , $300 'Entnommene Wh und Ws aus EEPROM lesen Led3 = 0 Waitms 150 Readeeprom Summe_mah_entladen , $310 Led4 = 0 Waitms 150 Readeeprom Summe_as_laden , $320 Led5 = 0 Waitms 150 Readeeprom Summe_mah_laden , $330 Led6 = 0 Waitms 150 Readeeprom Akkukap , $340 Led7 = 0 Waitms 150 Readeeprom Kilometer , $350 Readeeprom Meter , $360 Readeeprom Zentimeter , $370
'<Faktoren für den PID-Regler einlesen> P_faktor = 0.0033 'Readeeprom P_faktor , $380 I_faktor = 0.00006 'Readeeprom I_faktor , $384 D_faktor = 0.08 'Readeeprom D_faktor , $388 Mp_faktor = 0.006 'Readeeprom Mp_faktor , $38c Mv_faktor = 0 'Readeeprom Mv_faktor , $390 Mpl_faktor = 0 'Readeeprom Mpl_faktor , $394 Mvl_faktor = 0 'Readeeprom Mvl_faktor , $398
'Sinnvolle Werte vorbelegen If P_faktor < 0 Or P_faktor > 1 Then P_faktor = 0.0033 If I_faktor < 0 Or I_faktor > 1 Then I_faktor = 0.00006 If D_faktor < 0 Or D_faktor > 1 Then D_faktor = 0.08 If Mp_faktor < 0 Or Mp_faktor > 1 Then Mp_faktor = 0.006
If Mv_faktor < 0 Or Mv_faktor > 1 Then Mv_faktor = 0 If Mpl_faktor < 0 Or Mpl_faktor > 1 Then Mpl_faktor = 0 If Mvl_faktor < 0 Or Mvl_faktor > 1 Then Mvl_faktor = 0
'</Faktoren für den PID-Regler einlesen> If Kilometer = $ffff Then 'Kommt bei frischem EEPROM vor
Zentimeter = 0 Meter = 0 Kilometer = 0 Min_v = 0 Max_v = 0
End If 'Bei Fehlerhaften Werten aus dem EEPROM einfach 10000mAh als Vorgabe If Akkukap <= 0 Then Akkukap = 10000
- if Systemvoltage = 14 'Nur zum Test mit Netzteil
If Akkukap > 32000 Then Akkukap = 100
- endif
- if Systemvoltage = 24
If Akkukap > 32000 Then Akkukap = 100
- endif
- if Systemvoltage = 36
If Akkukap > 32000 Then Akkukap = 100
- endif
- if Systemvoltage = 48
If Akkukap > 32000 Then Akkukap = 100
- endif
Call Akkugrenzen Fusscount = 0 Notaus = 0 Readeeprom Max_v , $2f0 Readeeprom Min_v , $2f4 Config Watchdog = 128 'mSec
'Maximalwerte für Datenschreiber/Notauserkennung auf 0 setzen Regelzeit = 0 Regelzeitmax = 0 Mit_acc_nick_max = 0 Mit_acc_roll_max = 0 Pwm_rechts_max = 0 Pwm_links_max = 0 K_diff_min = 0 K_diff_max = 0 Richtung_links = 2 'Leerlauf Richtung_rechts = 2 'Leerlauf Pwm_links = 0 Pwm_rechts = 0 Pwm_gesamt = 0 Mit_acc_nick_s = 0 Lenkerstellung = 0
K_rechts = 0 K_links = 0 Seriell_err_links = 0 Seriell_err_rechts = 0 'Vrbelegen um zu sehen ob es einen Notaus ohne Grund gibt Ugrund = "----------------"
Disable Interrupts
'Timer wird vorgeladen um die gewünschte Zykluszeit nach 'Timerüberlauf zu erreichen 'Nachteil: Timer Count ist nicht exakt gleich der Zykluszeit On Timer0 Regel_interrupt Config Timer0 = Timer , Prescale = 1024 ' (1/20MHz) * 1024 = 51,2uS pro Tick Enable Timer0
Antriebaus = 1 'Beim Start ist immer alles aus Enable Interrupts Waitms 100
'< Sicherheitstest. CPU will Reset if one of these conditions are met Start Watchdog 'nach 128mS wird CPU Reset ausgelöst If Resetbtn = 0 Then Wait 10 If Antriebaus = 0 Then Wait 10 'If Ugemessen < 1000 Then Wait 10 '10V 'If Ugemessen > 5500 Then Wait 10 '55V Stop Watchdog '< Sicherheitstest
'Prüfung ob Akku gerade geladen:
- if Systemvoltage = 14
'Sensoren ausschalten wenn am Ladegerät oder zu Testzwecken 'If Ugemessen < 2200 Then ' Sensor_aus = 1 'Else ' Sensor_aus = 0 'End If Errorword.0 = 1
While Ugemessen < 1100 'Unterspannung oder am Ladegerät Call Flashled(0 , 50) Wend If Ugemessen > 1520 Then 'Akku ist ganzgeladen
- endif
- if Systemvoltage = 24
'Sensoren ausschalten wenn am Ladegerät oder zu Testzwecken 'If Ugemessen < 2200 Then ' Sensor_aus = 1 'Else ' Sensor_aus = 0 'End If Errorword.1 = 1 While Ugemessen < 2200 'Unterspannung oder am Ladegerät Call Flashled(0 , 50) Wend If Ugemessen > 2650 Then 'Akku ist ganzgeladen
- endif
- if Systemvoltage = 36
'Sensoren ausschalten wenn am Ladegerät oder zu Testzwecken 'If Ugemessen < 3300 Then ' Sensor_aus = 1 'Else ' Sensor_aus = 0 'End If Errorword.2 = 1 While Ugemessen < 3300 'Unterspannung oder am Ladegerät Call Flashled(0 , 50) Wend If Ugemessen > 4350 Then 'Akku ist ganzgeladen
- endif
- if Systemvoltage = 48
'Sensoren ausschalten wenn am Ladegerät oder zu Testzwecken 'If Ugemessen < 2400 Then ' Sensor_aus = 1 'Else ' Sensor_aus = 0 'End If Errorword.3 = 1 While Ugemessen < 4300 'Unterspannung oder am Ladegerät Call Flashled(0 , 50) Wend If Ugemessen > 5280 Then 'Akku ist ganzgeladen
- endif
'Interruts abschalten damit die folgenden Zeilen nicht gerade jetzt vom 'Interrupt unterbrochen werden. Disable Interrupts Waitms 50 Summe_as_entladen = 0 Summe_mah_entladen = 0 Summe_as_laden = 0 Summe_mah_laden = 0 Enable Interrupts End If
Led3 = 0 Led4 = 0 Led5 = 0 Led6 = 0 Led7 = 0
Goto Jp
'Summen_vorbelegen: Do
Errorword.4 = 1
' Acc_roll = Acc_roll * 100 ' Acc_nick = Acc_nick * 100
'Sum_gyro_roll = Gyro_roll * Gyro_anz 'Sum_gyro_nick = Gyro_nick * Gyro_anz 'Sum_gyro_gier = Gyro_gier * Gyro_anz
Gyro_roll_alt = Gyro_roll Gyro_nick_alt = Gyro_nick Gyro_gier_alt = Gyro_gier
Mit_gyro_roll = Gyro_roll Mit_gyro_nick = Gyro_nick Mit_gyro_gier = Gyro_gier
Sum_acc_roll = Acc_roll * Acc_anz Sum_acc_nick = Acc_nick * Acc_anz
Waitms 200 Toggle Led5 Toggle Led6 Waitms 200 Toggle Led5 Toggle Led6
Loop Until Sig_gyro_nick >= -3 And Sig_gyro_nick <= 3 And Sig_gyro_roll >= -3 And Sig_gyro_roll <= 3 And Sig_gyro_gier >= -3 And Sig_gyro_gier <= 3 Errorword.4 = 0
Jp:
Call Resetdata 'and Enable Interrupt Resetbtndone = 0
Do
Errorword.5 = 1 '<Sicherheitstest Sensoren> If Antriebaus = 1 Then If Intcount <> Ltest Then Ltest = Intcount Sensor_ok = 0 If Sensor_ok_count > 0 Then Decr Sensor_ok_count '1. Es darf keine nennenswerte Drehbewegung erkannt werden!
If Sig_gyro_nick < -3 Then Sensor_ok_count = 50 If Sig_gyro_nick > 3 Then Sensor_ok_count = 50 If Sig_gyro_roll < -3 Then Sensor_ok_count = 50 If Sig_gyro_roll > 3 Then Sensor_ok_count = 50 'Nach mehrfachen Drehungen könnte der Mittelwert etwas 'verschoben sein. Darum hier 12 If Sig_gyro_gier < -15 Then Sensor_ok_count = 50 If Sig_gyro_gier > 15 Then Sensor_ok_count = 50
'2. Die Werte dürfen nicht verrauscht sein Ldummy2 = Mit_acc_nick + 500 If Acc_nick > Ldummy2 Then Sensor_ok_count = 50 Ldummy2 = Mit_acc_nick - 500 If Acc_nick < Ldummy2 Then Sensor_ok_count = 50 Ldummy2 = Mit_acc_roll + 500 If Acc_roll > Ldummy2 Then Sensor_ok_count = 50 Ldummy2 = Mit_acc_roll - 500 If Acc_roll < Ldummy2 Then Sensor_ok_count = 50
'3. Die Werte müssen ungefähr in einer legalen Nullage sein. If Null_roll < O_roll_u Then Sensor_ok_count = 50 If Null_roll > O_roll_o Then Sensor_ok_count = 50 If Null_nick < O_nick_u Then Sensor_ok_count = 50 If Null_nick > O_nick_o Then Sensor_ok_count = 50
'Bedingungen während 50 Messungen (0.5s) eingehalten If Sensor_ok_count = 0 Then Sensor_ok = 1 End If Else Sensor_ok = 0 End If '</Sicherheitstest Sensoren>
'<Batterieanzeige> 'alles unter 36V Leerlaufspannung ist LEER If Ugemessen > Akkufastleer Then If Verbraucht > Akkukap Then 'Einfach mal annehmen daß noch mehr als angezeigt im Akku ist Akkukap = Verbraucht + 1 Call Akkugrenzen End If End If
If Antriebaus = 1 Then Errorword.6 = 1 Else Errorword.6 = 0 End If
If Notaus = 1 Then Errorword.7 = 1 Else Errorword.7 = 0 End If
'Blinken wenn Gyros nicht bereit If Antriebaus = 1 Then If Ugemessen > Akkuleer Then If Verbraucht > Akkukap Then 'annehmen dass noch mehr als angezeigt im Akku ist Akkukap = Verbraucht + 20 Call Akkugrenzen End If End If If Sensor_ok = 1 Then If Verbraucht < Akku1 Then Led7 = 0 Else Led7 = 1 If Verbraucht < Akku2 Then Led6 = 0 Else Led6 = 1 If Verbraucht < Akku3 Then Led5 = 0 Else Led5 = 1 If Verbraucht < Akku4 Then Led4 = 0 Else Led4 = 1 If Verbraucht < Akku5 Then Led3 = 0 Else Led3 = 1 Else If Frei = 1 Then 'ca. 2 Hz Blinkcount = Intcount If Blinkcount > 50 Then Blinkcount = Blinkcount - 50 Else Blinkcount = Intcount / 2 End If If Blinkcount > 40 Then 'ca. 1 Hz If Verbraucht < Akku1 Then Led7 = 0 Else Led7 = 1 If Verbraucht < Akku2 Then Led6 = 0 Else Led6 = 1 If Verbraucht < Akku3 Then Led5 = 0 Else Led5 = 1 If Verbraucht < Akku4 Then Led4 = 0 Else Led4 = 1 If Verbraucht < Akku5 Then Led3 = 0 Else Led3 = 1 Else Led7 = 1 Led6 = 1 Led5 = 1 Led4 = 1 Led3 = 1 End If End If Else Errorword.6 = 0 If Verbraucht < Akku1 Then Led7 = 0 Else Led7 = 1 If Verbraucht < Akku2 Then Led6 = 0 Else Led6 = 1 If Verbraucht < Akku3 Then Led5 = 0 Else Led5 = 1 If Verbraucht < Akku4 Then Led4 = 0 Else Led4 = 1 If Verbraucht < Akku5 Then Led3 = 0 Else Led3 = 1 End If '</Batterieanzeige>
'< Maximalwerte Für Betriebsdatenschreiber> If Igemessen < Imin Then Imin = Igemessen 'Nur zur Information If Igemessen > Imax Then Imax = Igemessen 'Nur zur Information If Regelzeit > Regelzeitmax Then Regelzeitmax = Regelzeit If Mit_acc_nick > Mit_acc_nick_max Then Mit_acc_nick_max = Mit_acc_nick If Mit_acc_roll > Mit_acc_roll_max Then Mit_acc_roll_max = Mit_acc_roll If Mit_acc_nick < Mit_acc_nick_min Then Mit_acc_nick_min = Mit_acc_nick If Mit_acc_roll < Mit_acc_roll_min Then Mit_acc_roll_min = Mit_acc_roll If Pwm_links > Pwm_links_max Then Pwm_links_max = Pwm_links If Pwm_rechts > Pwm_rechts_max Then Pwm_rechts_max = Pwm_rechts If K_diff > K_diff_max Then K_diff_max = K_diff If K_diff < K_diff_min Then K_diff_min = K_diff If Ta > Tamax Then Tamax = Ta '</Maximalwerte für Betriebsdatenschreiber>
'Debounce macht dasselbe, allerdings stockt der Programmfluss Debounce Resetbtn , 0 , Onresetbtn , Sub
Goto Skipit
If Resetbtndone = 0 Then If Resetbtn = 0 Then Resetbtndone = 1 Call Onresetbtn Resetbtndone = 2 Else End If Else If Resetbtndone = 2 Then Resetbtndone = 0 End If
Skipit:
If Intcount > 80 Then Led1 = 1 Else Led1 = 0 'Sekundenblinken 'Daran lässt sich leicht erkennen ob noch alles normal läuft.
Loop End
'********************************************************************** 'Ab hier Subroutinen und Interrupts '********************************************************************** Sub Akkugrenzen
'Grenzwerte für die Akku-LEDs berechnen Ldummy1 = Akkukap / 5 Akku1 = Ldummy1 Akku2 = 2 * Ldummy1 Akku3 = 3 * Ldummy1 Akku4 = 4 * Ldummy1 Akku5 = 5 * Ldummy1
End Sub
Sub Unfalldatenschreiber
Waitms 10 Utext = Str(regelzeit) Utext = "Rz=" + Utext Logtext 0 , Utext Utext = Str(mit_acc_nick_max) Utext = "Nick max=" + Utext Logtext $10 , Utext Utext = Str(mit_acc_nick) Utext = "Nick=" + Utext Logtext $20 , Utext Utext = Str(mit_acc_nick_min) Utext = "Nick min=" + Utext Logtext $30 , Utext Utext = Str(mit_acc_roll_max) Utext = "Roll max=" + Utext Logtext $40 , Utext Utext = Str(mit_acc_roll) Utext = "Roll=" + Utext Logtext $50 , Utext Utext = Str(mit_acc_roll_min) Utext = "Roll min=" + Utext Logtext $60 , Utext Utext = Str(pwm_rechts) Utext = "PWM re=" + Utext Logtext $70 , Utext Utext = Str(pwm_links) Utext = "PWM li=" + Utext Logtext $80 , Utext Utext = Str(u_rechts) Utext = "U re=" + Utext Logtext $90 , Utext Utext = Str(u_links) Utext = "U li=" + Utext Logtext $a0 , Utext Utext = Str(ta) Utext = "Ta=" + Utext Logtext $b0 , Utext Utext = Str(seriell_err_links) Utext = Utext + "|" Utext = Utext + Str(seriell_err_rechts) Utext = "S =" + Utext Logtext $c0 , Utext Utext = Ugrund Logtext $d0 , Utext
End Sub
Sub Betriebsdatenschreiber
'Ob die Wartezeiten beim EEprom schreiben nötig sind ist nicht sicher. 'Mit den Wartezeiten treten aber keine Fehler bei den Werten mehr auf. 'Möglich, daß dies auch durch eine andere Änderung bewirkt wurde. Writeeeprom Max_v , $2f0 Writeeeprom Min_v , $2f2 Zentimeter = Int(zentimeter) 'Keine Millimeter schreiben Writeeeprom Summe_as_entladen , $300 Writeeeprom Summe_mah_entladen , $310 Writeeeprom Summe_as_laden , $320 Writeeeprom Summe_mah_laden , $330 Writeeeprom Akkukap , $340 Writeeeprom Kilometer , $350 Writeeeprom Meter , $360 Writeeeprom Zentimeter , $370 If Werteneu = 1 Then 'Nur wenn das Einstellklavier gesteckt ist. Writeeeprom P_faktor , $380 Writeeeprom I_faktor , $384 Writeeeprom D_faktor , $388 Writeeeprom Mp_faktor , $38c Writeeeprom Mv_faktor , $390 Writeeeprom Mpl_faktor , $394 Writeeeprom Mvl_faktor , $398 Werteneu = 0 End If
Utext = Str(pwm_rechts_max) Utext = "PWM r max=" + Utext Logtext $e0 , Utext Utext = Str(pwm_links_max) Utext = "PWM l max=" + Utext Logtext $f0 , Utext Utext = Str(ugemessen) Utext = "U=" + Utext Logtext $100 , Utext Utext = Str(imin) Utext = "Imin=" + Utext Logtext $110 , Utext Utext = Str(imax) Utext = "Imax=" + Utext Logtext $120 , Utext Utext = Str(summe_mah_entladen) Utext = "-mAh" + Utext Logtext $130 , Utext Utext = Str(summe_mah_laden) Utext = "+mAh=" + Utext Logtext $140 , Utext Utext = Str(verbraucht) Utext = " =" + Utext Logtext $150 , Utext Utext = Str(akkukap) Utext = "Kap=" + Utext Logtext $160 , Utext Utext = Str(kilometer) Utext = "Km=" + Utext Logtext $170 , Utext Utext = Str(meter) Utext = "M=" + Utext Logtext $180 , Utext Utext = Str(zentimeter) Utext = "cm=" + Utext Logtext $190 , Utext Utext = Str(regelzeitmax) Utext = "Rz Max=" + Utext Logtext $1a0 , Utext Utext = Str(null_roll) Utext = "Null_R=" + Utext Logtext $1b0 , Utext Utext = Str(null_nick) Utext = "null_N=" + Utext Logtext $1c0 , Utext Utext = Str(p_faktor) Utext = "P=" + Utext Logtext $1d0 , Utext Utext = Str(i_faktor) Utext = "I=" + Utext Logtext $1e0 , Utext Utext = Str(d_faktor) Utext = "D=" + Utext Logtext $1f0 , Utext Utext = Str(mp_faktor) Utext = "M=" + Utext Logtext $200 , Utext Utext = Str(k_diff_min) Utext = "K min=" + Utext Logtext $210 , Utext Utext = Str(k_diff_max) Utext = "K max=" + Utext Logtext $220 , Utext Utext = Str(seriell_err_links) Utext = "S l=" + Utext Logtext $230 , Utext Utext = Str(seriell_err_rechts) Utext = "S r=" + Utext Logtext $240 , Utext Utext = Str(tamax) Utext = "Ta_Max=" + Utext Logtext $250 , Utext Utext = Str(max_v) Utext = "V_Max=" + Utext Logtext $260 , Utext Utext = Str(min_v) Utext = "V_Min=" + Utext Logtext $270 , Utext
End Sub
Sub Onresetbtn
If Antriebaus = 1 Then
'alles unter 36V Leerlaufspannung ist LEER
If Ugemessen > Akkuleer Then
If Sensor_ok = 1 Then 'Prüfen ob Gyros noch driften
'Interupts abschalten damit Fusscount nicht gerade jetzt
'decrementiert wird
Call Resetdata
Else
If Null_nick > O_nick_o And Null_nick < O_nick_oo Then
If Bzeit = 500 Then
Bstate = 1
Bzeit = 0
Else
If Bstate = 1 Then
If Bzeit < 50 Then
Disable Interrupts
Utext = " "
Logtext $00 , Utext
Logtext $10 , Utext
Logtext $20 , Utext
Logtext $30 , Utext
Logtext $40 , Utext
Logtext $50 , Utext
Logtext $60 , Utext
Logtext $70 , Utext
Logtext $80 , Utext
Logtext $90 , Utext
Logtext $a0 , Utext
Logtext $b0 , Utext
Logtext $c0 , Utext
Logtext $d0 , Utext
Logtext $e0 , Utext
Logtext $f0 , Utext
Logtext $100 , Utext
Logtext $110 , Utext
Logtext $120 , Utext
Logtext $130 , Utext
Logtext $140 , Utext
Logtext $150 , Utext
Logtext $160 , Utext
Logtext $170 , Utext
Logtext $180 , Utext
Logtext $190 , Utext
Logtext $1a0 , Utext
Logtext $1b0 , Utext
Logtext $1c0 , Utext
Logtext $1d0 , Utext
Logtext $1e0 , Utext
Logtext $1f0 , Utext
Logtext $200 , Utext
Logtext $210 , Utext
Logtext $220 , Utext
Logtext $230 , Utext
Logtext $240 , Utext
Logtext $250 , Utext
Logtext $260 , Utext
Summe_as_entladen = 0
Summe_mah_entladen = 0
Summe_as_laden = 0
Summe_mah_laden = 0
Writeeeprom Summe_as_entladen , $300
Writeeeprom Summe_mah_entladen , $310
Writeeeprom Summe_as_laden , $320
Writeeeprom Summe_mah_laden , $330
Frei = 0
For Bzeit = 1 To 10
Led3 = 1
Led4 = 1
Led5 = 1
Led6 = 1
Led7 = 1
Waitms 100
Led3 = 0
Led4 = 0
Led5 = 0
Led6 = 0
Led7 = 0
Waitms 100
Next
Bzeit = 500
Enable Interrupts
Waitms 100
End If
If Bzeit > 250 And Bzeit < 350 Then
Frei = 1
Bzeit = 500
Else
Frei = 0
Bzeit = 500
End If
End If
End If
Else
Frei = 0
Bzeit = 500
End If
End If
Else
If Verbraucht < Akkukap Then 'Akku ist laut Spannung leer!
'Einfach mal annehmen daß zuviel entnommen wurde
Akkukap = Verbraucht * 0.95
Akkugrenzen
End If
'Disable Interrupts
'Betriebsdatenschreiber
'Enable Interrupts
End If
Else
Antriebaus = 1
Waitms 100
Frei = 0
'Disable Interrupts 'Betriebsdatenschreiber 'Enable Interrupts End If
End Sub
Sub Logtext(adresse , Text)
If Len(text) > 16 Then Text = Mid(text , 1 , 16) While Len(text) < 16 Text = Text + " " Wend Writeeeprom Text , Adresse
End Sub
Regel_interrupt: '***************************************************************************** 'Eigentlich ist das kein typischer Interrupt. Der Interrupt wird hier nur 'verwendet um der Regelung einen 100Hz Takt zu geben. 'Andere Timer Interrupts werden für diese Zeit blockiert '***************************************************************************** 'Timer zählt ab diesem Wert aufwärts bis 256 und macht dann einen Interrupt '==> je höher die Startzahl, umso kürzer das Intervall ' 20 MHz / 1024 (Prescaler) = 15,625 KHz / 256 = 61Hz ' (61/100) * 256 = 156 ' 256 - 156 = 100 Der Timer selbst braucht auch etwas Zeit, d.h. 100 ist tatsächlich 61 Timer0 = 61
Bdummy1 = Udr
Gosub Readfussschalter Gosub Read_gyro Gosub Read_acc
'Acc_roll = Acc_roll '*100, war früher -512...+512 jetzt -30000...+30000 'Acc_nick = Acc_nick '*100, war früher -512...+512 jetzt -30000...+30000
'<Mittellage ausgleichen> 'Die Offsetwerte werden langsam an eine früher ermittelte Waagerechte 'Position angeglichen. Null_roll = Null_roll * 99 Null_nick = Null_nick * 99 Null_roll = Null_roll + Offset_roll 'Mittellage Null_nick = Null_nick + Offset_nick 'Mittellage Null_roll = Null_roll / 100 Null_nick = Null_nick / 100 '</Mittellage ausgleichen>
'Offsetwerte der Beschleunigungssensoren sofort abziehen 'Diese werden nicht laufend ermittelt 'Acc_roll = Acc_roll - Null_roll 'Acc_nick = Acc_nick - Null_nick '<Geschwindikgeitsbegrenzung> 'Bei Der Geschwindigkeitsbegrenzung Wird die Nullage Der Plattform Verändert. 'Beim Vorwärtsbremsen wird nach hinten geneigt, beim Rückwärtsbremsen nach vorne.
Vbrems = 0 Lbrems = 0 If Frei = 0 Then
If Kmh > Vmax Then Vbrems = Vmax - Kmh If Kmh < Vmaxr Then Vbrems = Vmaxr - Kmh
End If
'Lastabhängige Bremse 'Es muss immer genug Reserve sein um noch zu Bremsen oder zu Balancieren Ldummyi = Int(pwm_gesamt) If Ldummyi > Spwm Then Lbrems = Spwm - Ldummyi If Ldummyi < Spwmr Then Lbrems = Spwmr - Ldummyi Brems = Lbrems + Vbrems Brems = Brems * 50 'Nicht zu heftig Bremsen If Brems < -2000 Then Brems = -2000 If Brems > 2000 Then Brems = 2000 Acc_nick = Acc_nick + Brems '</Geschwindikgeitsbegrenzung>
'<Signalaufbereitung der Sensorsignale> K_diff = K_rechts - K_links K_sum = K_sum * 0.84 K_sum = K_sum + K_diff 'Gleitender Mittelwert der Gyros berechnen um damit den Offset bei 'Stillstand zu haben. Dieser ist leider Temperaturabhängig. 'Sum_gyro_gier = Sum_gyro_gier - Mit_gyro_gier 'Sum_gyro_gier = Sum_gyro_gier + Gyro_gier 'Mit_gyro_gier = Sum_gyro_gier / Gyro_anz Sig_gyro_gier = Gyro_gier '- Mit_gyro_gier 'Polarität
'Sum_gyro_roll = Sum_gyro_roll - Mit_gyro_roll 'Sum_gyro_roll = Sum_gyro_roll + Gyro_roll 'Mit_gyro_roll = Sum_gyro_roll / Gyro_anz Sig_gyro_roll = Gyro_roll '- Mit_gyro_roll 'Polarität
'Korr_acc_roll = Sig_gyro_roll * Gyro_faktor Sum_acc_roll = Sum_acc_roll - Mit_acc_roll Sum_acc_roll = Sum_acc_roll + Acc_roll 'Sum_acc_roll = Sum_acc_roll + Korr_acc_roll 'Gyrowert hinzu 'Mittelwert bilden Mit_acc_roll = Sum_acc_roll / Acc_anz Mit_acc_roll = Sum_acc_roll
'************************************************************************* 'Das soll ausgleichen, daß die Gyros bei gekippten Sensoren eine Drehung 'um die falsche Achse registrieren. 'Sdummy1 = Mit_acc_roll * Sig_gyro_gier 'Die Sensorplatine muss dazu absolut Waagerecht montiert sein. 'Sdummy1 = Sdummy1 / 20000 'Lagefehler = Int(sdummy1) 'Das soll den Fehler ausgleichen, der durch die geänderte Lage des 'Gyros auftritt. 'Gyro_nick = Gyro_nick + Lagefehler '************************************************************************ Sum_gyro_nick = Sum_gyro_nick - Mit_gyro_nick Sum_gyro_nick = Sum_gyro_nick + Gyro_nick Mit_gyro_nick = Sum_gyro_nick / Gyro_anz Sig_gyro_nick = Gyro_nick - Mit_gyro_nick
'Gyro = Beschleunigung 'Accelerometer = Neigung
'Korr_acc_nick = Sig_gyro_nick * Gyro_faktor Sum_acc_nick = Sum_acc_nick - Mit_acc_nick Sum_acc_nick = Sum_acc_nick + Acc_nick 'Sum_acc_nick = Sum_acc_nick - Korr_acc_nick 'Gyrowert hinzu Mit_acc_nick = Sum_acc_nick / Acc_anz '</Signalaufbereitung der Sensorsignale>
'<PID-Regler> Stellwert = 0 '< P-Anteil> Sdummy1 = Mit_acc_nick Sdummy1 = Sdummy1 * P_faktor Stellwert = Stellwert - Sdummy1
'<I-Anteil>
Mit_acc_nick_s = Mit_acc_nick_s + Mit_acc_nick 'Überlauf/unsinnige Werte verhindern If Mit_acc_nick_s > Max_mit_acc_nick_s Then Mit_acc_nick_s = Max_mit_acc_nick_s End If If Mit_acc_nick_s < Min_mit_acc_nick_s Then Mit_acc_nick_s = Min_mit_acc_nick_s End If Sdummy1 = Mit_acc_nick_s Sdummy1 = Sdummy1 * I_faktor Stellwert = Stellwert - Sdummy1
'<D-Anteil>
Sdummy1 = Sig_gyro_nick Sdummy1 = Sdummy1 * D_faktor 'Stellwert ==> Stromsollwert Stellwert = Stellwert + Sdummy1
'<Anpassungsfaktoren berechnen> K_sum_anpassung = K_rechts + K_links K_sum_anpassung = Abs(k_sum_anpassung)
Mp_anpassung = Pwm_gesamt * Mp_faktor Mp_anpassung = Abs(mp_anpassung) Mp_anpassung = Mp_anpassung + 0.95
Mv_anpassung = K_sum_anpassung * Mv_faktor Mv_anpassung = Abs(mv_anpassung) Mv_anpassung = Mv_anpassung + 1
Mpl_anpassung = Pwm_gesamt * Mpl_faktor Mpl_anpassung = Abs(mpl_anpassung) Mpl_anpassung = Mpl_anpassung + 1
Mvl_anpassung = K_sum_anpassung * Mvl_faktor Mvl_anpassung = Abs(mvl_anpassung) Mvl_anpassung = Mvl_anpassung + 1 '</Anpassungsfaktoren berechnen>
'<Verstärkte Regelung bei hoher Leistung> Stellwert = Stellwert * Mp_anpassung Stellwert = Stellwert * Mv_anpassung '<Verstärkte Regelung bei hoher Leistung>
'<Abgeschwächte Regelung im Stand> If K_sum_anpassung < 8 Then Stellwert = Stellwert * 0.8 If K_sum_anpassung < 6 Then Stellwert = Stellwert * 0.8 If K_sum_anpassung < 4 Then Stellwert = Stellwert * 0.8
'</Abgeschwächte Regelung in Stand> Pwm_gesamt = Pwm_gesamt + Stellwert
'</PID-Regler>
'<Bremsvibrator einmischen> 'Der Bremsvibrator signalisiert dem Fahrer, daß wegen irgend einem 'Grenzwert gebremst wird. Das kann V-Max oder P-Max sein. If Brems <> 0 Then
Bdummy1 = Intcount / 2 Bdummy1 = Bdummy1 Mod 2 If Bdummy1 = 0 Then Pwm_gesamt = Pwm_gesamt + 5 Else Pwm_gesamt = Pwm_gesamt - 5 End If
End If '<Bremsvibrator einmischen>
'<Lenkung> Lenkvorgabe = Mit_acc_roll / 80
Sdummy1 = Abs(lenkvorgabe) If Sdummy1 <= 6 Then
Sdummy1 = Sdummy1 / 12 Lenkerstellung = Lenkvorgabe * Sdummy1
Else
If Lenkvorgabe < 0 Then Lenkerstellung = Lenkvorgabe + 3 Else Lenkerstellung = Lenkvorgabe - 3 End If
End If
'Lenkfehler berechnen und zum Stellwert hinzu. Sdummy1 = Sig_gyro_gier Sdummy1 = Sdummy1 / 5 Lenkfehler = Lenkerstellung - Sdummy1 Lenkfehler = Lenkfehler * Mpl_anpassung Lenkfehler = Lenkfehler * Mvl_anpassung
Lenkstellwert = Lenkerstellung + Lenkfehler
If Lenkstellwert > Max_lenk Then Lenkstellwert = Max_lenk If Lenkstellwert < Min_lenk Then Lenkstellwert = Min_lenk
Pwm_lenk_links = Pwm_gesamt + Lenkstellwert Pwm_lenk_rechts = Pwm_gesamt - Lenkstellwert
If Pwm_lenk_links > 0 Then Richtung_links = 1 Else Richtung_links = 0 If Pwm_lenk_rechts > 0 Then Richtung_rechts = 1 Else Richtung_rechts = 0
Pwm_lenk_links = Abs(pwm_lenk_links) Pwm_lenk_rechts = Abs(pwm_lenk_rechts)
If Pwm_lenk_links > Maxpwm Then Pwm_lenk_links = Maxpwm If Pwm_lenk_rechts > Maxpwm Then Pwm_lenk_rechts = Maxpwm
If Pwm_lenk_links < Minpwm Then Pwm_lenk_links = Minpwm If Pwm_lenk_rechts < Minpwm Then Pwm_lenk_rechts = Minpwm
Pwm_links = Pwm_lenk_links Pwm_rechts = Pwm_lenk_rechts '</Lenkung>
If Antriebaus = 1 Then
If Notaus = 1 Then 'Unfalldatenschreiber: ************************************************* 'Dauert zwar Lange, macht aber nix. Bei Notaus muss nix geregelt werden. ' call Unfalldatenschreiber ' call Betriebsdatenschreiber Notaus = 0 '*********************************************************************** End If 'Auf 0 setzen was sonst geregelt wird Richtung_links = 2 'Leerlauf Richtung_rechts = 2 'Leerlauf Pwm_links = 0 Pwm_rechts = 0 Pwm_gesamt = 0 Mit_acc_nick_s = 0 Lenkerstellung = 0 K_rechts = 0 K_links = 0 'Waagerechte Positoion setzen wenn Zustand Antriebaus: Wdummy1 = Acc_roll Null_roll = Wdummy1 * 100 Wdummy1 = Acc_nick Null_nick = Wdummy1 * 100 'Mittelwert der Beschleunigungssensoren auf aktuellen Wert stellen 'Der Mittelwert braucht sonst zu lange bis er sich eingeregelt hat Sum_acc_roll = Acc_roll * Acc_anz Sum_acc_nick = Acc_nick * Acc_anz If Intcount = 0 Then T30 = 1 'Regelparameter vom Tacho anfordern If Intcount = 50 Then T30 = 1 'Regelparameter vom Tacho anfordern
End If
'****************************************************************************** If T30 = 1 Then 'Regelparameter vom Tacho anfordern
K_links = 0
K_rechts = 0
Zeiger_meldung = 1
Bdummy1 = Udr
T30 = 0 'Damit keine alte Rückmeldung ausgewertet wird
Timeout = Timer0
Timeout = Timeout + 100
'Anforderung an Display senden
Print Chr(255) ; Chr(0) ; Chr(0) ; Chr(255) ; Chr(13)
While Zeiger_meldung < 31
If Ucsr0a.rxc0 = 1 Then 'Warten bis ein Byte eingetroffen ist
Tachomeldung(zeiger_meldung) = Udr
Incr Zeiger_meldung
Timeout = Timer0
Timeout = Timeout + 20
End If
If Timer0 > Timeout Then
T30 = 0
Exit While 'Timeout
End If
Wend
If T30 = 13 Then 'Jetzt BCC berechnen
!lds R24,{t1}
!LDs R25,{t2}
!add R24,R25
!LDs R25,{t3}
!add R24,R25
!LDs R25,{t4}
!add R24,R25
!LDs R25,{t5}
!add R24,R25
!LDs R25,{t6}
!add R24,R25
!LDs R25,{t7}
!add R24,R25
!LDs R25,{t8}
!add R24,R25
!LDs R25,{t9}
!add R24,R25
!LDs R25,{t10}
!add R24,R25
!LDs R25,{t11}
!add R24,R25
!LDs R25,{t12}
!add R24,R25
!LDs R25,{t13}
!add R24,R25
!LDs R25,{t14}
!add R24,R25
!LDs R25,{t15}
!add R24,R25
!LDs R25,{t16}
!add R24,R25
!LDs R25,{t17}
!add R24,R25
!LDs R25,{t18}
!add R24,R25
!LDs R25,{t19}
!add R24,R25
!LDs R25,{t20}
!add R24,R25
!LDs R25,{t21}
!add R24,R25
!LDs R25,{t22}
!add R24,R25
!LDs R25,{t23}
!add R24,R25
!LDs R25,{t24}
!add R24,R25
!LDs R25,{t25}
!add R24,R25
!LDs R25,{t26}
!add R24,R25
!LDs R25,{t27}
!add R24,R25
!LDs R25,{t28}
!add R24,R25
!STS {Bdummy1},R24
If Bdummy1 = T29 Then P_faktor = P_tacho I_faktor = I_tacho D_faktor = D_tacho Mp_faktor = Mp_tacho Mv_faktor = Mv_tacho Mpl_faktor = Mpl_tacho Mvl_faktor = Mvl_tacho Werteneu = 1 End If End If
Else
'Errechnete Geschwindigkeiten an Motoren Senden:
'Rechter Motor:
If Nummotors = 2 Then
Checksum_rechts = 82
Checksum_rechts = Checksum_rechts + Richtung_rechts
Checksum_rechts = Checksum_rechts + Pwm_rechts
'Geschwindigkeit an Motorregler senden
Bdummy1 = Udr 'Puffer leeren
'Print Chr(82); Chr(richtung_rechts); Chr(pwm_rechts); Chr(checksum_rechts); Chr(13);
'In Assembler geht das so:
!Ldi R24 , 82
!STS udr,R24
Warten1:
!LDS R24,UCSR0A
!BST R24,5
!Brtc warten1
!LDS R24,{Richtung_rechts}
!STS udr,R24
Warten2:
!LDS R24,UCSR0A
!BST R24,5
!Brtc warten2
!lds R24,{Pwm_rechts} 'PWM Wert
!STS udr,R24
Warten3:
!LDS R24,UCSR0A
!BST R24,5
!Brtc warten3
!LDS R24,{Checksum_rechts}
!STS udr,R24
Warten4:
!LDS R24,UCSR0A
!BST R24,5
!Brtc warten4
!LDI R24,13
!STS udr,R24
Warten5:
!LDS R24,UCSR0A
!BST R24,5
!Brtc warten5
Motormeldung(8) = 0 'Damit keine alte Rückmeldung ausgewertet wird
'Vom Motorcontroller lesen Timeout = Timer0 Zeiger_meldung = 1 '80 Ticks * 51,2uS pro Tick = 4mS Wartezeit auf Antwort 'COM benötigt ca. 1,5mS für 9 Byte Errorword.8 = 0 Timeout = Timeout + 80 While Zeiger_meldung < 9 If Ucsr0a.rxc0 = 1 Then 'Warten bis ein Byte eingetroffen ist Motormeldung(zeiger_meldung) = Udr Incr Zeiger_meldung End If If Timer0 > Timeout Then Ende = 0 Errorword.8 = 1 Exit While 'Timeout End If Wend
!lds R24,{M1}
!LDs R25,{M2}
!ADD R24,R25
!LDs R25,{M3}
!ADD R24,R25
!LDs R25,{M4}
!ADD R24,R25
!LDs R25,{M5}
!ADD R24,R25
!LDs R25,{M6}
!ADD R24,R25
!STS {Bdummy1},R24
If Bdummy1 <> Checksumme Then Ende = 0 If Ende = 13 Then U_rechts = U I_rechts = I K_rechts = K Else Incr Seriell_err_rechts S_err = 1 End If End If
'Linker Motor:
Checksum_links = 76 'Adresse
Checksum_links = Checksum_links + Richtung_links
Checksum_links = Checksum_links + Pwm_links
Zeiger_meldung = 1
Bdummy1 = Udr 'Puffer leeren
'Print Chr(76); Chr(richtung_links); Chr(pwm_links); Chr(checksum_links); Chr(13);
'>>>>>>>>>>>> Message an Motorcontroller senden <<<<<<<<<<<<<<
'In Assembler geht das so:
!Ldi R24 , 76 'Adresse
!STS udr,R24
Warten6:
!LDS R24,UCSR0A
!BST R24,5
!Brtc warten6
!LDS R24,{Richtung_links}
!STS udr,R24
Warten7:
!LDS R24,UCSR0A
!BST R24,5
!Brtc warten7
!lds R24,{Pwm_links}
!STS udr,R24
Warten8:
!LDS R24,UCSR0A
!BST R24,5
!Brtc warten8
!LDS R24,{Checksum_links}
!STS udr,R24
Warten9:
!LDS R24,UCSR0A
!BST R24,5
!Brtc warten9
!LDI R24,13
!STS udr,R24
Warten10:
!LDS R24,UCSR0A
!BST R24,5
!Brtc warten10
Motormeldung(8) = 0 'Damit keine alte Rückmeldung ausgewertet wird
Timeout = Timer0
Timeout = Timeout + 80 'Wartezeit auf Antwort
'>>>>>>>>>>>> Message vom Motorcontroller empfangen <<<<<<<<<<<<<<
While Zeiger_meldung < 9
If Ucsr0a.rxc0 = 1 Then 'Warten bis ein Byte eingetroffen ist
Motormeldung(zeiger_meldung) = Udr
Incr Zeiger_meldung
End If
If Timer0 > Timeout Then
Ende = 0
Errorword.8 = 1
Exit While 'Timeout
End If
Wend
!lds R24,{M1}
!LDs R25,{M2}
!ADD R24,R25
!LDs R25,{M3}
!ADD R24,R25
!LDs R25,{M4}
!ADD R24,R25
!LDs R25,{M5}
!ADD R24,R25
!LDs R25,{M6}
!ADD R24,R25
!STS {Bdummy1},R24
If Bdummy1 <> Checksumme Then Ende = 0 If Ende = 13 Then U_links = U I_links = I K_links = K Else Incr Seriell_err_links S_err = 1 End If If S_err = 1 Then 'Sicherheitshalber, damit der Lenkausgleich nicht überreagiert. K_links = 0 K_rechts = 0 S_err = 0 End If End If
If Antriebaus = 0 Then '<Not-Aus Kriterien ermitteln> If Mit_acc_nick > -300 Then If Mit_acc_nick < 300 Then Regelzeit = 0 Incr Regelzeit '</Not-Aus Kriterien ermitteln>
'<Not-Aus Kriterien abprüfen> If Regelzeit > 250 Then 'Wenn nach 2,5s waagerechte Lage nicht erreicht Notaus = 1 Antriebaus = 1 Ugrund = Str(regelzeit) Ugrund = "*Rz " + Ugrund End If Ldummyi = 4000 + Brems If Mit_acc_nick > Ldummyi Then 'zu weit nach hinten gekippt Notaus = 1 Antriebaus = 1 Ugrund = Str(mit_acc_nick) Ugrund = "*Nick " + Ugrund End If Ldummyi = -4000 + Brems If Mit_acc_nick < Ldummyi Then 'zu weit nach vorn gekippt Notaus = 1 Antriebaus = 1 Ugrund = Str(mit_acc_nick) Ugrund = "*Nick " + Ugrund End If If Mit_acc_roll > 5000 Then 'Lenker zu weit nach rechts Notaus = 1 Antriebaus = 1 Ugrund = Str(mit_acc_roll) Ugrund = "*Roll " + Ugrund End If If Mit_acc_roll < -5000 Then 'Lenker zu weit nach links Notaus = 1 Antriebaus = 1 Ugrund = Str(mit_acc_roll) Ugrund = "*Roll " + Ugrund End If If K_diff > 10 Then 'Plattform rotiert zu schnell Notaus = 1 Antriebaus = 1 Ugrund = Str(k_diff) Ugrund = "*Gier " + Ugrund End If If K_diff < -10 Then 'Plattform rotiert zu schnell Notaus = 1 Antriebaus = 1 Ugrund = Str(k_diff) Ugrund = "*Gier " + Ugrund End If '</Not-Aus Kriterien abprüfen> Else If Fusscount > 0 Then Antriebaus = 0 End If
'<Akkuspannung berechnen>
If Nummotors = 2 Then Ugemessen = U_links + U_rechts Shift Ugemessen , Right , 1 , Signed 'Div by 2 weil 2 Motoren Else Ugemessen = U_links End If '</Akkuspannung berechnen>
'<Gesamtstrom berechnen> If Nummotors = 2 Then Igemessen = I_links + I_rechts Else Igemessen = I_links End If '</Gesamtstrom berechnen>
'<verbrauchte Amperestunden berechnen> If Igemessen > 0 Then Summe_as_entladen = Summe_as_entladen + Igemessen 'Verbrauchte End If If Igemessen < 0 Then Summe_as_laden = Summe_as_laden - Igemessen 'Rückgespeiste End If 'mAh aus den Amperestunden rausholen um Überläufe zu verhindern 'Die 360000 setzen sich zusammen aus: 'Igemessen in mA -> Faktor 1 '100 Messungen pro Sekunde -> mal 100 '1 mAh = 60 Sekunden * 60 mAsekunden -> 3600 '360000 = 1 * 100 * 3600 If Summe_as_entladen > 360000 Then Incr Summe_mah_entladen Summe_as_entladen = Summe_as_entladen - 360000 End If If Summe_as_laden > 360000 Then 'Das kann passieren wenns den Berg runter geht Incr Summe_mah_laden Summe_as_laden = Summe_as_laden - 360000 End If Erzeugt = Summe_mah_laden Verbraucht = Summe_mah_entladen Erzeugt = Erzeugt * 0.7 'Das ist der Akku-Wirkungsgrad 0.7 (0.85) bezogen auf den Stom Verbraucht = Verbraucht - Erzeugt Akkuinhalt = Akkukap Akkuverbrauch = Verbraucht Akkuprozent = Akkuverbrauch / Akkuinhalt Akkuprozent = Akkuprozent * 1000 Akkuprozent = Int(akkuprozent) Akkustand = Akkuprozent '</verbrauchte As berechnen>
'<Strecke berechnen> Kx = K_links + K_rechts Kx = Kx / 2 Strecke = Kx * Di Zentimeter = Zentimeter + Strecke If Zentimeter >= 100 Then Incr Meter Zentimeter = Zentimeter - 100 End If If Zentimeter < 0 Then Decr Meter Zentimeter = Zentimeter + 100 End If If Meter >= 1000 Then Incr Kilometer Meter = Meter - 1000 End If If Meter < 0 Then Decr Kilometer Meter = Meter + 1000 End If '</Strecke berechnen>
'<Geschwindigkeit berechnen> Kv = Kv + Kx Kmh = Kv * Kmhfaktor Kv = Kv * 0.75 'Bei Änderung auch den kmhfaktor neu berechnen If Kmh > Max_v Then Max_v = Kmh If Kmh < Min_v Then Min_v = Kmh '</Geschwindigkeit berechnen>
If Bzeit < 500 Then Incr Bzeit Incr Intcount
'****************************************************************************** 'ADiverse Werte werden an den Tacho geschickt. Manche für die Funktion 'des Tachos, andere um die Steuerung zu optimieren. '****************************************************************************** Incr Tachocount
Select Case Tachocount
Case 0 B(1) = 40 'Error Word B(2) = Errorword1 B(3) = Errorword2 B(5) = 1 Case 1 B(1) = 1 'U U = Ugemessen B(2) = Motormeldung(1) B(3) = Motormeldung(2) B(5) = 1 Case 2 B(1) = 2 'I I = Igemessen / 10 B(2) = Motormeldung(3) B(3) = Motormeldung(4) B(5) = 1 Case 3 B(1) = 3 'Km/h B(2) = Kmhmeldung(1) B(3) = Kmhmeldung(2) B(5) = 1 Case 4 B(1) = 5 'Akkuprozente B(2) = Akkumeldung(1) B(3) = Akkumeldung(2) B(5) = 1 Case 5 B(1) = 7 'Meter B(2) = Metermeldung(1) B(3) = Metermeldung(2) B(5) = 1 Case 6 B(1) = 6 'Kilometer B(2) = Kilometermeldung(1) B(3) = Kilometermeldung(2) B(5) = 1 Case 7 If Antriebaus = 0 Then B(1) = 8 'Low B(2) = Mit_acc_roll1 B(3) = Mit_acc_roll2 B(5) = 1 Else B(1) = 12 'Low B(2) = Null_roll1 B(3) = Null_roll2 B(5) = 1 End If Case 8 If Antriebaus = 0 Then B(1) = 9 'High B(2) = Mit_acc_roll3 B(3) = Mit_acc_roll4 B(5) = 1 Else B(1) = 13 'High B(2) = Null_roll3 B(3) = Null_roll4 B(5) = 1 End If Case 9 B(1) = 4 'Status B(2) = Antriebaus B(3) = Ta B(5) = 1 Case 10 If Antriebaus = 0 Then B(1) = 10 'acc_Nick B(2) = Mit_acc_nick1 B(3) = Mit_acc_nick2 B(5) = 1 Else B(1) = 14 B(2) = Null_nick1 B(3) = Null_nick2 B(5) = 1 End If Case 11 'Nick und Roll If Antriebaus = 0 Then B(1) = 11 'acc_Nick B(2) = Mit_acc_nick3 B(3) = Mit_acc_nick4 B(5) = 1 Else B(1) = 15 B(2) = Null_nick3 B(3) = Null_nick4 B(5) = 1 End If Case 12 B(1) = 16 'Stellwert B(2) = Stellwert1 B(3) = Stellwert2 B(5) = 1 Case 13 B(1) = 17 'Stellwert B(2) = Stellwert3 B(3) = Stellwert4 B(5) = 1 Case 14 Gyronick = Sig_gyro_nick B(1) = 18 'Gyro Nick_l B(2) = Gyronick1 B(3) = Gyronick2 B(5) = 1 Case 15 B(1) = 19 'Gyro Nick_h B(2) = Gyronick3 B(3) = Gyronick4 B(5) = 1 Case 16 Kksum = K_sum B(1) = 20 '??_l B(2) = Kksum1 B(3) = Kksum2 B(5) = 1 Case 17 B(1) = 21 '??_h B(2) = Kksum3 B(3) = Kksum4 B(5) = 1 If Antriebaus = 0 Then Tachocount = 255 'Gleich zu 0 überlaufen lassen Case 18 B(1) = 22 B(2) = Pf1 B(3) = Pf2 B(5) = 1 Case 19 B(1) = 23 B(2) = Pf3 B(3) = Pf4 B(5) = 1 Case 20 B(1) = 24 B(2) = If1 B(3) = If2 B(5) = 1 Case 21 B(1) = 25 B(2) = If3 B(3) = If4 B(5) = 1 Case 22 B(1) = 26 B(2) = Df1 B(3) = Df2 B(5) = 1 Case 23 B(1) = 27 B(2) = Df3 B(3) = Df4 B(5) = 1 Case 24 B(1) = 28 B(2) = Mpf1 B(3) = Mpf2 B(5) = 1 Case 25 B(1) = 29 B(2) = Mpf3 B(3) = Mpf4 B(5) = 1 Case 26 B(1) = 30 B(2) = Mvf1 B(3) = Mvf2 B(5) = 1 Case 27 B(1) = 31 B(2) = Mvf3 B(3) = Mvf4 B(5) = 1 Case 28 B(1) = 32 B(2) = Mplf1 B(3) = Mplf2 B(5) = 1 Case 29 B(1) = 33 B(2) = Mplf3 B(3) = Mplf4 B(5) = 1 Case 30 B(1) = 34 B(2) = Mvlf1 B(3) = Mvlf2 B(5) = 1 Case 31 B(1) = 35 B(2) = Mvlf3 B(3) = Mvlf4 B(5) = 1 Case 32 B(1) = 36 'Port Status B(2) = Portc B(3) = Porta B(5) = 1 Case 33 B(1) = 37 'Fussschalter 1 Wwert = Adc0 'Word B(2) = Wwert1 B(3) = Wwert2 B(5) = 1 Case 34 B(1) = 38 'Fussschalter 2 Wwert = Adc1 'Word B(2) = Wwert1 B(3) = Wwert2 B(5) = 1 Case 35 B(1) = 39 'Fussschalter gesamt B(2) = Fussschalter B(3) = 0 B(5) = 1 Tachocount = 255 Case Else B(5) = 0 'Nichts ausgeben
End Select
If Intcount = 99 Then Intcount = 255
If B(5) = 1 Then
'Checksumme berechnen
B(4) = B(1)
B(4) = B(4) + B(2)
B(4) = B(4) + B(3)
'Print Chr(b(1)) ; Chr(b(2)) ; Chr(b(3)) ; Chr(b(4)) ; Chr(13);
'Print geht so in Assembler:
!LDS R24,{B(1)} 'Adresse von b(1)
!STS udr,R24
Warten11:
!LDS R24,UCSR0A
!BST R24,5
!Brtc warten11
!LDS R24,{B(2)} 'Adresse von b(2)
!STS udr,R24
Warten12:
!LDS R24,UCSR0A
!BST R24,5
!Brtc warten12
!lds R24,{B(3)} 'Adresse von B(3)
!STS udr,R24
Warten13:
!LDS R24,UCSR0A
!BST R24,5
!Brtc warten13
!LDS R24,{B(4)} 'Adresse von B(4)
!STS udr,R24
Warten14:
!LDS R24,UCSR0A
!BST R24,5
!Brtc warten14
!LDI R24,13 'CR
!STS udr,R24
Warten15:
!LDS R24,UCSR0A
!BST R24,5
!Brtc warten15
Ta = Timer0 'Um festzustellen ob der Interrupt zu lange dauert
'Wenn Ja, ist der Zählwert nahe 255
End If Return
Sub Flashled(byval Which As Byte , Byval Dauer_ms As Byte)
Dim Done As Byte Dim Waitstarttime As Byte Dim Xx As Byte
Select Case Which Case 0 'Both Led1 = 1 Led2 = 1 Waitms Dauer_ms Led1 = 0 Led2 = 0 Waitms Dauer_ms Case 1 'LED1 Led1 = 1 Waitms Dauer_ms Led1 = 0 Waitms Dauer_ms Case 2 'LED2 Led2 = 1 Waitms Dauer_ms Led2 = 0 Waitms Dauer_ms End Select
End Sub
Readfussschalter:
Adc0 = Getadc(0) If Adc0 > Fussschalterschwelle Then Fussschalter.0 = 1 Else Fussschalter.0 = 0 End If
Adc1 = Getadc(1) If Adc1 > Fussschalterschwelle Then Fussschalter.1 = 1 Else Fussschalter.1 = 0 End If
If Fussschalter = 0 Then 'Max 0.5 Sekunden ohne Fussschalter If Fusscount > 0 Then Decr Fusscount Else Fusscount = 50 'alles OK End If
If Fusscount = 0 Then If Antriebaus = 0 Then Antriebaus = 1 Waitms 100 'Damit der Regel_interrupt auch was davon mitbekommt 'Disable Interrupts 'Betriebsdatenschreiber 'Enable Interrupts End If End If
Return
Read_gyro: 'Beschleunigung
I2cstart I2cwbyte Gyroadress_w 'Start address for Read I2cwbyte &B10101000 'Out_x_l AND &B10000000 I2cstart I2cwbyte Gyroadress_r 'I2crbyte Temperatur , Ack 'I2crbyte Status , Ack
I2crbyte Gyro_roll1 , Ack I2crbyte Gyro_roll2 , Ack
I2crbyte Gyro_nick1 , Ack I2crbyte Gyro_nick2 , Ack
I2crbyte Gyro_gier1 , Ack I2crbyte Gyro_gier2 , Nack I2cstop
Return
Read_acc: 'Neigung
I2cstart I2cwbyte Accadress_w 'Start address for Read I2cwbyte &B10101000 'Out_x_l_a AND &B10000000 I2cstart I2cwbyte Accadress_r
'I2crbyte Status_a , Ack
I2crbyte Accroll_l , Ack I2crbyte Accroll_h , Ack Acc_roll = Accroll Acc_roll = Acc_roll / 2 'Divisor = Empfindlichkeit
I2crbyte Accnick_l , Ack I2crbyte Accnick_h , Ack Acc_nick = Accnick Acc_nick = Acc_nick / 10 'Divisor = Empfindlichkeit
I2crbyte Accgier_l , Ack I2crbyte Accgier_h , Nack Acc_gier = Accgier Acc_gier = Acc_gier / 10 'Divisor = Empfindlichkeit I2cstop
Return
'Get_mag_data: ' Slaveadress = Magadress ' Subadress = Out_x_h_m ' Gosub Read_6_mag
' Xmag = Dataarray(1) ' Shift Xmag , Left , 8 ' Xmag = Xmag + Dataarray(2)
' Ymag = Dataarray(3) ' Shift Ymag , Left , 8 ' Ymag = Ymag + Dataarray(4)
' Zmag = Dataarray(5) ' Shift Zmag , Left , 8 ' Zmag = Zmag + Dataarray(6)
' Xmag = Xmag - Xmagoff ' Ymag = Ymag - Ymagoff 'Return
Init_gyro:
'refer to MiniMu9 L3G4200D_Gyro.pdf, page 29 Slaveadress = Gyroadress_w Subadress = Ctrl_reg1 '01 ==> 200Hz ' 11 ==> Bandwidth = 70 ' 0 ==> Power Down Mode ' 1 ==> Z-Axis enable ' 1 ==> Y-Axis enable ' 1 ==> X-Axis enable Databyte = &B00001111 'Enable x, y, z and turn off power down I2cstart I2cwbyte Slaveadress I2cwbyte Subadress I2cwbyte Databyte I2cstop Subadress = Ctrl_reg2
Databyte = &B0010_1001 'adjust/use the HPF I2cstart I2cwbyte Slaveadress I2cwbyte Subadress I2cwbyte Databyte I2cstop
Subadress = Ctrl_reg3 Databyte = &B0000_1000 I2cstart I2cwbyte Slaveadress I2cwbyte Subadress I2cwbyte Databyte I2cstop
Subadress = Ctrl_reg4 '1 ==> Block Data Update ' 0 ==> BLE ' 00 ==> 250 dps ' 0 ==> not used ' 00 ==> Self Test 0=Normal Mode ' 0 ==> SIM 0=4-wire interface Databyte = &B10000000 I2cstart I2cwbyte Slaveadress I2cwbyte Subadress I2cwbyte Databyte I2cstop
Subadress = Ctrl_reg5 Databyte = &B0000_0000 'high-pass filtering of outputs I2cstart I2cwbyte Slaveadress I2cwbyte Subadress I2cwbyte Databyte I2cstop
Return
Init_acc:
Slaveadress = Accadress_w Subadress = Ctrl_reg1_a '001 ==> Normal Mode ' 01 ==> 100Hz, Bandwith=74 ' 1 ==> Z-Axis enable ' 1 ==> Y-Axis enable ' 1 ==> X-Axis enable Databyte = &B00101111 I2cstart I2cwbyte Slaveadress I2cwbyte Subadress I2cwbyte Databyte I2cstop
Slaveadress = Accadress_w Subadress = Ctrl_reg4_a '1 ==> Continuous, update when finished ' 0 ==> BLE ' 00 ==> FSE = 2g ' 0 ==> Self Test Sign ' 0 ==> not used ' 0 ==> Self Test Enable ' 0 ==> not used Databyte = &B10000000 'Block Data Update, LSB low, Scale +-2g I2cstart I2cwbyte Slaveadress I2cwbyte Subadress I2cwbyte Databyte I2cstop
Return
'Init_mag: ' Slaveadress = Magadress ' Subadress = Cra_reg_m ' Databyte = &B00011000 '75Hz Output ' I2cstart ' I2cwbyte Slaveadress ' I2cwbyte Subadress ' I2cwbyte Databyte ' I2cstop
' Slaveadress = Magadress ' Subadress = Crb_reg_m ' Databyte = &B01000000 'range +-1.9Gauss ' I2cstart ' I2cwbyte Slaveadress ' I2cwbyte Subadress ' I2cwbyte Databyte ' I2cstop
' Slaveadress = Magadress ' Subadress = Mr_reg_m ' Databyte = &B00000000 'continious mode ' I2cstart ' I2cwbyte Slaveadress ' I2cwbyte Subadress ' I2cwbyte Databyte ' I2cstop 'Return
Sub Resetdata
Disable Interrupts Regelzeit = 0 Regelzeitmax = 0 Mit_acc_nick_max = 0 Mit_acc_roll_max = 0 Mit_acc_nick_min = 0 Mit_acc_roll_min = 0 Pwm_rechts_max = 0 Pwm_links_max = 0 Seriell_err_rechts = 0 Seriell_err_links = 0 K_diff_min = 0 K_diff_max = 0 Antriebaus = 0 Fusscount = 50 '0,5 Sekunden Enable Interrupts
End Sub
Display3000:
'****************Listrik01 LCD Display Steuerung Version 01 ***************** $hwstack = 256 $swstack = 256 $framesize = 64 $regfile = "m2561def.dat" $crystal = 16000000 'microcontroller crystal frequency $baud1 = 57600
'(start block comment
--------------------- | | | --------------- | | | | | | | | | | | | | | | | | | | | | | | | | | | | | | | | | Port D.0 = Tageskilometerzähler Reset | | | | | | | | | | --------------| | Display: | | 1| 2| |3| | 1 = Zyklusdauer (0..255) | --------------- | 2 = Motorleistung links | | 3 = Motorleistung rechts | | | O1 O2 | | O5 | Schalter: | | 1: PinD.7 Zeile abwärts | O7 O8 | 2: PinD.4 Zeile aufwärts | | 3: PinE.5 Blättern | O6 | 4: PinE.7 SD-Karte aktivieren | O3 O4 | 5: PinG.0 Werte übernehmen | | 6: PinG.3 Taste defekt | | 7: PinG.1 Zahl kleiner | | 8: PinD.5 Zahl größer | | 9: Cpu Reset | | | | | O9 | ---------------------
')end block comment
'####################### LCD-Routines ######################################
'Definition of used ports and pull up resistors. 'We are using Port B for the SPI-communication to the LCD Display.
Ddra = &B00000000 'switch all 8 Ports of Port A to input (0),
'Pin (PA.0 - PA.7)
Porta = &B11111111 'All port pins have individually selectable pull-up
'resistors. Here we enable these pull-up-resisitors,
'so these Pins are always at logical 1
'You Need To Pull These Pins Against Ground(gnd)
'Select Port B as to an output port (data output to the display) Ddrb = &B01110110 'DDR = Data direction register; Port B1, B2, B4, B5, B6
'switched to output (1) as needed by the display
Portb = &B10001001 'the other ports of Port B are inputs with
'switched on pull up resistors
Ddrc = &B00000000 'switch all Ports of Port C to input Portc = &B11111111 'all pull-up-Resistors turned on
Ddrd = &B00000000 'switchall Ports of Port D to input Portd = &B11110011 'almost all pull-up-Resistors turned on
Ddre = &B10000000 'E7=Output (SD card activate), all other Input Porte = &B11111111 'all pull-up-Resistors turned on
Ddrf = &B00000000 'switch all Ports of Port F to input Portf = &B11111111 'all pull-up-Resistors turned on
Ddrg = &B00000000 'switch all Ports of Port G to input Portg = &B11111111 'all pull-up-Resistors turned on '#############################################################################
Declare Sub Activate_display_bus Declare Sub Activate_sd_bus() Declare Sub Minmax() Declare Function Trimtext(t As String) As String * 22 Declare Sub Drawpagestatics
Const Titel = "Tacho 0.1 "
Dim Ep0 As Byte At $0201 Dim Ep1 As Byte At $0202 Dim Ep2 As Byte At $0203 Dim Ep3 As Byte At $0204 Dim Ep4 As Byte At $0205 Dim Ep5 As Byte At $0206 Dim Iwert As Integer At $0203 Overlay Dim Wwert As Integer At $0203 Overlay Dim Index As Byte At $202 Overlay
Dim Maccx As Long At $0207 Dim Maccx1 As Byte At $0207 Overlay Dim Maccx2 As Byte At $0208 Overlay Dim Maccx3 As Byte At $0209 Overlay Dim Maccx4 As Byte At $020a Overlay Dim Maccy As Long At $020b Dim Maccy1 As Byte At $020b Overlay Dim Maccy2 As Byte At $020c Overlay Dim Maccy3 As Byte At $020d Overlay Dim Maccy4 As Byte At $020e Overlay
Dim Nullx As Long At $020f Dim Nullx1 As Byte At $020f Overlay Dim Nullx2 As Byte At $0210 Overlay Dim Nullx3 As Byte At $0211 Overlay Dim Nullx4 As Byte At $0212 Overlay Dim Nully As Long At $0213 Dim Nully1 As Byte At $0213 Overlay Dim Nully2 As Byte At $0214 Overlay Dim Nully3 As Byte At $0215 Overlay Dim Nully4 As Byte At $0216 Overlay Dim Sw As Single At $00217
Dim Sw1 As Byte At $0217 Overlay Dim Sw2 As Byte At $0218 Overlay Dim Sw3 As Byte At $0219 Overlay Dim Sw4 As Byte At $021a Overlay
Dim Gyronick As Long At $0021b Dim Gyronick1 As Byte At $021b Overlay Dim Gyronick2 As Byte At $021c Overlay Dim Gyronick3 As Byte At $021d Overlay Dim Gyronick4 As Byte At $021e Overlay
Dim Kksum As Single At $0021f Dim Kksum1 As Byte At $021f Overlay Dim Kksum2 As Byte At $0220 Overlay Dim Kksum3 As Byte At $0221 Overlay Dim Kksum4 As Byte At $0222 Overlay
Dim P_faktor As Single At $0223 Dim P_f1 As Byte At $0223 Overlay Dim P_f2 As Byte At $0224 Overlay Dim P_f3 As Byte At $0225 Overlay Dim P_f4 As Byte At $0226 Overlay
Dim I_faktor As Single At $0227 Dim I_f1 As Byte At $0227 Overlay Dim I_f2 As Byte At $0228 Overlay Dim I_f3 As Byte At $0229 Overlay Dim I_f4 As Byte At $022a Overlay
Dim D_faktor As Single At $022b Dim D_f1 As Byte At $022b Overlay Dim D_f2 As Byte At $022c Overlay Dim D_f3 As Byte At $022d Overlay Dim D_f4 As Byte At $022e Overlay
Dim Mp_faktor As Single At $022f Dim Mp_f1 As Byte At $022f Overlay Dim Mp_f2 As Byte At $0230 Overlay Dim Mp_f3 As Byte At $0231 Overlay Dim Mp_f4 As Byte At $0232 Overlay
Dim Mv_faktor As Single At $0233 Dim Mv_f1 As Byte At $0233 Overlay Dim Mv_f2 As Byte At $0234 Overlay Dim Mv_f3 As Byte At $0235 Overlay Dim Mv_f4 As Byte At $0236 Overlay
Dim Mpl_faktor As Single At $0237 Dim Mpl_f1 As Byte At $0237 Overlay Dim Mpl_f2 As Byte At $0238 Overlay Dim Mpl_f3 As Byte At $0239 Overlay Dim Mpl_f4 As Byte At $023a Overlay
Dim Mvl_faktor As Single At $023b Dim Mvl_f1 As Byte At $023b Overlay Dim Mvl_f2 As Byte At $023c Overlay Dim Mvl_f3 As Byte At $023d Overlay Dim Mvl_f4 As Byte At $023e Overlay
Dim Pfaktor As Long Dim Ifaktor As Long Dim Dfaktor As Long Dim Mpfaktor As Long Dim Mvfaktor As Long Dim Mplfaktor As Long Dim Mvlfaktor As Long
Dim Stellwert As Single Dim Gyro_nick As Long Dim Null_y As Long Dim Null_x As Long Dim Mit_acc_y As Long Dim Mit_acc_x As Long Dim K_sum As Single Dim K_summax As Single Dim K_summin As Single
Dim Bdummy As Byte Dim Singdummy As Single Dim Wdummy As Word Dim Ldummy As Long Dim Idummy As Integer Dim Kilometer As Word Dim Meter As Integer Dim Akkuprozente As Integer Dim Kmh As Integer Dim Kmhmax As Integer Dim Kmhmin As Integer
Dim I As Integer Dim Imax As Integer Dim Imin As Integer
Dim U As Integer Dim Umax As Integer Dim Umin As Integer Dim Ml As Integer Dim Mlmax As Integer Dim Mrmax As Integer Dim Mr As Integer Dim Mlmin As Integer Dim Mrmin As Integer Dim Modus As Byte Dim Editzeile As Byte Dim Utext As String * 22 Dim Itext As String * 22 Dim Ltext As String * 22 Dim L1 As Byte Dim L2 As Byte Dim L3 As Byte Dim Text As String * 22 Dim Farbe As Word Dim X1 As Byte Dim Y1 As Byte Dim X2 As Byte Dim Y2 As Byte Dim Startkilometer As Single Dim Ta As Byte Dim Tamax As Byte
Dim Antriebaus As Byte
Dim Xoffset As Long Dim Yoffset As Long Dim Ya As Byte Dim Xa As Byte
'Dim Lname As Long 'Dim Dateiname As String * 12 'Dim Dateilaenge As Long 'Dim Dateiaktuell As Byte Dim Gbdriveerror As Byte Dim Ulog As Single Dim Ilog As Single Dim Kmhlog As Single Dim Kmlog As Single Dim Akkuprozentelog As Single Dim Loopcount As Word Dim Aloopcount As Word Dim Logtext As String * 255 Dim Zeit As String * 8 Dim Sichern As Byte Dim Transferok As Byte
Dim Hexstr As String * 2
Dim Hauptstportc As Byte Dim Hauptstporta As Byte Dim Ad0 As Word 'AD value 0...1024 Dim Ad1 As Word 'AD value 0...1024 Dim Fussschalter As Byte 'Bit0 und Bit1 Dim Dummybyte As Byte
Config Clock = User Config Date = Dmy , Separator = .
'No Need To Call Here Because Init21_display3000.bas Has Already Done This Call Activate_display_bus
$include "Y:\Programme\Tools\BASCOM-AVR\Display 3000\Displaymodul\Bascom Programme\für Include\Init21_display3000.bas"
'If Porte.7 = 0 Then Activate_sd_bus
'Gbdriveerror = Driveinit() 'Init MMC/SD Card
'If Gbdriveerror = 0 Then
' Include AVR-DOS Configuration and library
' $include "Y:\Programme\Tools\BASCOM-AVR\Display 3000\SD Speicherkartenmodul P001\Bascom Basic\Config_AVR-DOS.BAS"
' Bdummy = Initfilesystem(1) ' Partition 1
' Ldummy = Disksize()
' Ldummy = Ldummy / 1024
' Utext = Str(ldummy)
' Utext = "Size: " + Utext
' Utext = Utext + " mb"
' Ldummy = Diskfree()
' Ldummy = Ldummy / 1024
' Itext = Str(ldummy)
' Itext = "Free: " + Itext
' Itext = Itext + " mb"
' If Porte.7 = 1 Then call Activate_display_bus
' gosub Lcd_cls
' call Lcd_print(Titel , 0 , 0 , 1 , 1 , 1 , Yellow , Blue)
' call Lcd_print ("SD-Karte init" , 0 , 18 , 1 , 1 , 1 , Black , White)
' call Lcd_print (Utext , 0 , 27 , 1 , 1 , 1 , Black , White)
' Lcd_print Itext , 0 , 36 , 1 , 1 , 1 , Black , White
' Wait 7
' Else
' If Porte.7 = 1 Then call Activate_display_bus
' Ltext = Str(gbdriveerror)
' Ltext = "Fehler: " + Ltext
' gosub Lcd_cls
' call Lcd_print (Titel , 0 , 0 , 1 , 1 , 1 , Yellow , Blue)
' call Lcd_print ("SD-Karte init" , 0 , 18 , 1 , 1 , 1 , Black , White)
' call Lcd_print (Ltext , 0 , 27 , 1 , 1 , 1 , Black , White)
' Wait 7
' End If
Readeeprom Startkilometer , 1 Readeeprom Xoffset , 5 Readeeprom Yoffset , 9
'Einige Werte vorbelegen um fehlerhafte Grafik zu vermeiden wenn keine 'serielle Verbindung besteht Xa = 74 Ya = 130 Antriebaus = 1 Null_x = Xoffset Null_y = Yoffset
'If Porte.7 = 1 Then call Activate_display_bus Orientation = Portrait180 'landscape mode Graphics_mode = 65k_uncompressed 'select the needed color mode, here 65.536 colors Call Lcd_cls
Call Drawpagestatics
Config Timer2 = Timer , Prescale = 128 Assr.exclk = 0 Assr.as2 = 1
On Timer2 Tick Enable Timer2 Start Timer2
On Urxc1 Datenempfang Enable Urxc1 Enable Interrupts Minmax Do
If Pine.5 = 0 Then While Pine.5 = 0 : Wend 'Debounce Button Incr Modus If Gbdriveerror <> 0 Then 'Logging nicht zulassen wenn DriveError If Modus > 5 Then Modus = 0 end if end If
If Modus <> 4 Then'nur berechnen wenn die Werte nicht editiert werden Singdummy = P_faktor * 10000 Singdummy = Round(Singdummy) Pfaktor = Singdummy
Singdummy = I_faktor * 1000000 Singdummy = Round(Singdummy) Ifaktor = Singdummy
Singdummy = D_faktor * 1000 Singdummy = Round(Singdummy) Dfaktor = Singdummy
Singdummy = Mp_faktor * 10000 Singdummy = Round(Singdummy) Mpfaktor = Singdummy
Singdummy = Mv_faktor * 1000 Singdummy = Round(Singdummy) Mvfaktor = Singdummy
Singdummy = Mpl_faktor * 10000 Singdummy = Round(Singdummy) Mplfaktor = Singdummy
Singdummy = Mvl_faktor * 1000 Singdummy = Round(Singdummy) Mvlfaktor = Singdummy End If
If Modus > 5 Then Modus = 0 'If Porte.7 = 1 Then call Activate_display_bus Call Lcd_cls Call Drawpagestatics End If If Modus <> 5 Then 'Nur wenn nicht geloggt wird If U <> 0 Then 'Nur wenn auch schon eine Spannung übertragen wurde If Ta <> 0 Then Idummy = U 'Nur wenn auch schon ein Ta übertragen wurde If Idummy > Umax Then Umax = Idummy If Idummy < Umin Then Umin = Idummy Idummy = I If Idummy > Imax Then Imax = Idummy If Idummy < Imin Then Imin = Idummy Idummy = Kmh If Idummy > Kmhmax Then Kmhmax = Idummy If Idummy < Kmhmin Then Kmhmin = Idummy Bdummy = Ta If Bdummy > Tamax Then Tamax = Bdummy Idummy = Ml If Idummy > Mlmax Then Mlmax = Idummy If Idummy < Mlmin Then Mlmin = Idummy Idummy = Mr If Idummy > Mrmax Then Mrmax = Idummy If Idummy < Mrmin Then Mrmin = Idummy End If End If End If
Select Case Modus
Case 0 'If Dateiaktuell = 1 Then ' If Porte.7 = 0 Then Activate_sd_bus ' Close #2 ' Dateiaktuell = 0 'End If 'If Porte.7 = 1 Then call Activate_display_bus 'Standard Anzeige If Pind.0 = 0 Then Waitms 3 If Pind.0 = 0 Then While Pind.0 = 0 : Wend Singdummy = Meter / 1000 Singdummy = Singdummy + Kilometer Startkilometer = Singdummy Writeeeprom Startkilometer , 1 End If End If
'Spannung und Strom Singdummy = U / 100 Utext = Fusing(singdummy , "#.#") Utext = Utext + "V" Utext = " " + Utext Singdummy = I / 100 Itext = Fusing(singdummy , "#.#") Itext = Itext + "A " L1 = Len(utext) L2 = Len(itext) L1 = L1 + L2 L2 = 22 - L1 Ltext = String(l2 , 32) Text = Utext + Ltext Text = Text + Itext Lcd_print Text , 0 , 75 , 1 , 1 , 1 , Black , White
'Akku in Prozent Singdummy = Akkuprozente / 10 Singdummy = 100 - Singdummy Text = Fusing(singdummy , "#.#" ) Text = " " + Text + "% " If Singdummy > 66 Then Farbe = Green Elseif Singdummy > 33 Then Farbe = Yellow Else Farbe = Red End If Singdummy = Singdummy * 1.14 '124 - 8 - 1 - 1 = 114 / 100% = 1.14 X2 = Singdummy 'Type cast X2 = X2 + 10 '10 wegen der "100 - Sdummy" weiter oben Lcd_box X2 , 85 , 123 , 94 , White 'Hintergrund löschen Lcd_box 9 , 85 , X2 , 94 , Farbe 'Balken zeichnen 'Text mittig platzieren L1 = Len(text) L1 = L1 * 5 L1 = L1 / 2 X1 = 66 - L1 Lcd_print Text , X1 , 98 , 1 , 1 , 1 , Black , White 'unterm Balken
'km/h Singdummy = Kmh / 10 Text = Fusing(Singdummy , "#.#") Text = Text + "km/h " Text = " " + Text L1 = Len(text) Text = Text + " " L1 = L1 * 4 X1 = 66 - L1 Lcd_print Text , X1 , 12 , 2 , 1 , 2 , Black , White
'km Singdummy = Meter / 1000 Singdummy = Singdummy + Kilometer Text = Fusing(Singdummy , "#.#") Text = Text + "km " Text = " " + Text L1 = Len(text) L1 = L1 * 5 L1 = L1 / 2 X1 = 66 - L1 Lcd_print Text , X1 , 46 , 1 , 1 , 1 , Black , White
'Tageskilometerzähler Singdummy = Singdummy - Startkilometer Text = Fusing(Singdummy , "#.###") Text = Text + "km " Text = " " + Text L1 = Len(text) L1 = L1 * 5 L1 = L1 / 2 X1 = 66 - L1 Lcd_print Text , X1 , 60 , 1 , 1 , 1 , Black , White
If Antriebaus = 1 Then Text = "AUS" Else Text = "RUN" End If 'Text mittig platzieren L1 = Len(text) L1 = L1 * 5 'Schriftlänge in Pixel, jedes Zeichen = 5 Pixel L1 = L1 / 2 'Halbe schriftlänge X1 = 74 - L1 'Text mittig Lcd_print Text , X1 , 148 , 1 , 1 , 1 , Black , White
'>>>>>>>>>>>> Fusszeile im Display >>>>>>>>>>>> ' 01234567891111111111222 ' 0123456789012 ' xxx xxx xxx 'PmotLinks Ta PmotRechts ' L1 L2 L3
Text = String(22 , 32) 'Fill with spaces
Utext = Str(ml) 'Pmot_links (Integer) L1 = Len(utext) Mid(text , 0 , L1) = Utext 'insert Ml
Utext = Str(ta) 'Ticks (Byte) L2 = Len(utext) Mid(text , 10 , L2) = Utext 'insert Ta
Utext = Str(mr) 'Pmot_rechts (Integer) L3 = Len(utext) X1 = 23 - L3 'Starting position of Mr Mid(text , X1 , L3) = Utext 'insert Mr
'L2 = X1 + L3 'L1 = 22 - L2 'Utext = String(l1 , 32) 'Mid(text , L2 , L1) = Utext 'insert final spaces
Lcd_print Text , 0 , 168 , 1 , 1 , 1 , White , Black
'>>>>>>>>>>>> Fusszeile im Display >>>>>>>>>>>>
If Antriebaus = 1 Then Singdummy = Null_x Singdummy = Singdummy - Xoffset Singdummy = Singdummy / 50 If Singdummy > 49 Then Singdummy = 49 If Singdummy < -49 Then Singdummy = -49 Singdummy = Singdummy + 74 X1 = Int(Singdummy) Lcd_draw Xa , 126 , Xa , 134 , 0 , White Lcd_draw X1 , 126 , X1 , 134 , 0 , Black Xa = X1 Singdummy = Null_y Singdummy = Singdummy - Yoffset Singdummy = Singdummy / 50 If Singdummy > 29 Then Singdummy = 29 If Singdummy < -29 Then Singdummy = -29 Singdummy = Singdummy + 130 Y1 = Int(Singdummy) Lcd_draw 9 , Ya , 18 , Ya , 0 , White Lcd_draw 9 , Y1 , 18 , Y1 , 0 , Black Ya = Y1 Else 'Horizontaler Balken Singdummy = Mit_acc_x Singdummy = Singdummy / 50 If Singdummy > 49 Then Singdummy = 49 If Singdummy < -49 Then Singdummy = -49 Singdummy = Singdummy + 74 X1 = Int(Singdummy) Lcd_draw Xa , 126 , Xa , 134 , 0 , White Lcd_draw X1 , 126 , X1 , 134 , 0 , Black Xa = X1
'Vertikaler Balken
Singdummy = Mit_acc_y
Singdummy = Singdummy / 50
If Singdummy > 29 Then Singdummy = 29
If Singdummy < -29 Then Singdummy = -29
Singdummy = Singdummy + 130
Y1 = Int(Singdummy)
Lcd_draw 9 , Ya , 18 , Ya , 0 , White
Lcd_draw 9 , Y1 , 18 , Y1 , 0 , Black
Ya = Y1
End If
Case 1 'Details
'If Dateiaktuell = 1 Then
' If Porte.7 = 0 Then Activate_sd_bus
' Close #2
' Dateiaktuell = 0
'End If
'If Porte.7 = 1 Then call Activate_display_bus
If Pind.0 = 0 Then
Waitms 3
If Pind.0 = 0 Then
While Pind.0 = 0 : Wend
If Antriebaus = 1 Then
Yoffset = Null_y
Xoffset = Null_x
Writeeeprom Xoffset , 5
Writeeeprom Yoffset , 9
End If
End If
End If
Singdummy = U / 100
Text = Fusing(Singdummy , "#.#")
Text = " U= " + Text
Text = Text + "V"
Text = Trimtext(text)
Lcd_print Text , 0 , 9 , 1 , 1 , 1 , Black , White
Singdummy = I / 100 Text = Fusing(Singdummy , "#.#") Text = " I= " + Text Text = Text + "A" Text = Trimtext(text) Lcd_print Text , 0 , 18 , 1 , 1 , 1 , Black , White
Singdummy = Kmh / 10 Text = Fusing(Singdummy , "#.#") Text = " V= " + Text Text = Text + "km/h" Text = Trimtext(text) Lcd_print Text , 0 , 27 , 1 , 1 , 1 , Black , White
Singdummy = Meter / 1000 Singdummy = Singdummy + Kilometer Text = Fusing(Singdummy , "#.###") Text = " S= " + Text Text = Text + "km " Text = Trimtext(text) Lcd_print Text , 0 , 36 , 1 , 1 , 1 , Black , White
Singdummy = Ml Text = Str(ml ) Text = " Pl= " + Text Text = Trimtext(text) Lcd_print Text , 0 , 45 , 1 , 1 , 1 , Black , White
Singdummy = Mr Text = Str(mr ) Text = " Pr= " + Text Text = Trimtext(text) Lcd_print Text , 0 , 54 , 1 , 1 , 1 , Black , White
Singdummy = Akkuprozente / 10 Singdummy = 100 - Singdummy Text = Fusing(Singdummy , "#.#") Text = "Akku= " + Text Text = Text + "%" Text = Trimtext(text) Lcd_print Text , 0 , 63 , 1 , 1 , 1 , Black , White
Text = Str(mit_acc_x) Text = "Roll= " + Text Text = Trimtext(text) Lcd_print Text , 0 , 72 , 1 , 1 , 1 , Black , White
Text = Str(mit_acc_y ) Text = "Nick= " + Text Text = Trimtext(text) Lcd_print Text , 0 , 81 , 1 , 1 , 1 , Black , White
Text = Str(null_x ) Text = " R0= " + Text Text = Trimtext(text) Lcd_print Text , 0 , 90 , 1 , 1 , 1 , Black , White
Text = Str(null_y ) Text = " N0= " + Text Text = Trimtext(text) Lcd_print Text , 0 , 99 , 1 , 1 , 1 , Black , White
Text = Fusing(stellwert , "#.###") Text = " SW= " + Text Text = Trimtext(text) Lcd_print Text , 0 , 108 , 1 , 1 , 1 , Black , White
Text = Str(gyro_nick ) Text = " GN= " + Text Text = Trimtext(text) Lcd_print Text , 0 , 117 , 1 , 1 , 1 , Black , White
Text = Str(ta ) Text = " TA= " + Text Text = Trimtext(text) Lcd_print Text , 0 , 126 , 1 , 1 , 1 , Black , White
Text = Str(k_sum) Text = "Ksum= " + Text Text = Trimtext(text) Lcd_print Text , 0 , 135 , 1 , 1 , 1 , Black , White Case 2 'Min/Max 'If Dateiaktuell = 1 Then ' If Porte.7 = 0 Then Activate_sd_bus ' Close #2 ' Dateiaktuell = 0 'End If 'If Porte.7 = 1 Then call Activate_display_bus
If Pind.0 = 0 Then Waitms 3 If Pind.0 = 0 Then While Pind.0 = 0 : Wend Minmax 'Min-Maxwerte auf 0 setzen End If End If Singdummy = Umin / 100 Text = Fusing(Singdummy , "#.#") Text = " Umin= " + Text Text = Text + "V" Text = Trimtext(text) Lcd_print Text , 0 , 9 , 1 , 1 , 1 , Black , White Singdummy = Umax / 100 Text = Fusing(Singdummy , "#.#") Text = " Umax= " + Text Text = Text + "V" Text = Trimtext(text) Lcd_print Text , 0 , 18 , 1 , 1 , 1 , Black , White '************************************************** Singdummy = Imin / 100 Text = Fusing(Singdummy , "#.#") Text = " Imin= " + Text Text = Text + "A" Text = Trimtext(text) Lcd_print Text , 0 , 27 , 1 , 1 , 1 , Black , White Singdummy = Imax / 100 Text = Fusing(Singdummy , "#.#") Text = " Imax= " + Text Text = Text + "A" Text = Trimtext(text) Lcd_print Text , 0 , 36 , 1 , 1 , 1 , Black , White '************************************************** Singdummy = Kmhmin / 10 Text = Fusing(Singdummy , "#.#") Text = " Vmin= " + Text Text = Text + "km/h" Text = Trimtext(text) Lcd_print Text , 0 , 45 , 1 , 1 , 1 , Black , White Singdummy = Kmhmax / 10 Text = Fusing(Singdummy , "#.#") Text = " Vmax= " + Text Text = Text + "km/h" Text = Trimtext(text) Lcd_print Text , 0 , 54 , 1 , 1 , 1 , Black , White '************************************************** Text = Str(mlmin ) Text = " Plmin= " + Text Text = Trimtext(text) Lcd_print Text , 0 , 63 , 1 , 1 , 1 , Black , White Text = Str(mlmax ) Text = " Plmax= " + Text Text = Trimtext(text) Lcd_print Text , 0 , 72 , 1 , 1 , 1 , Black , White '************************************************** Text = Str(mlmin ) Text = " Prmin= " + Text Text = Trimtext(text) Lcd_print Text , 0 , 81 , 1 , 1 , 1 , Black , White Text = Str(mrmax ) Text = " Prmax= " + Text Text = Trimtext(text) Lcd_print Text , 0 , 90 , 1 , 1 , 1 , Black , White '************************************************** Text = Str(tamax ) Text = " TAmax= " + Text Text = Trimtext(text) Lcd_print Text , 0 , 99 , 1 , 1 , 1 , Black , White '************************************************** Text = Bin(hauptstportc) Text = " HauptSt_C= " + Text Text = Trimtext(text) Lcd_print Text , 0 , 108 , 1 , 1 , 1 , Black , White '************************************************** Text = Bin(hauptstporta) Text = " HauptSt_A= " + Text Text = Trimtext(text) Lcd_print Text , 0 , 118 , 1 , 1 , 1 , Black , White '************************************************** Text = Str(ad0) Text = " AD(0)= " + Text Text = Trimtext(text) Lcd_print Text , 0 , 127 , 1 , 1 , 1 , Black , White '************************************************** Text = Str(ad1) Text = " AD(1)= " + Text Text = Trimtext(text) Lcd_print Text , 0 , 136 , 1 , 1 , 1 , Black , White '************************************************** Dummybyte = Fussschalter And &H1 If Dummybyte > 0 Then Text = "1" Else Text = "0" End If Text = " Fuss1= " + Text Text = Trimtext(text) Lcd_print Text , 0 , 145 , 1 , 1 , 1 , Black , White '************************************************** Dummybyte = Fussschalter And &H2 If Dummybyte > 0 Then Text = "1" Else Text = "0" End If Text = " Fuss2= " + Text Text = Trimtext(text) Lcd_print Text , 0 , 153 , 1 , 1 , 1 , Black , White Case 3 'Werte editieren 'If Dateiaktuell = 1 Then ' If Porte.7 = 0 Then Activate_sd_bus ' Close #2 ' Dateiaktuell = 0 'End If 'If Porte.7 = 1 Then call Activate_display_bus
If Pind.7 = 0 Then 'Zeile abwärts Waitms 3 If Pind.7 = 0 Then While Pind.7 = 0 : Wend If Editzeile > 1 Then Decr Editzeile : End If End If
If Pind.4 = 0 Then 'Zeile aufwärts Waitms 3 If Pind.4 = 0 Then While Pind.4 = 0 : Wend If Editzeile < 7 Then Incr Editzeile : End If End If
If Ping.1 = 0 Then 'Zahl dekrementieren
Waitms 3
If Ping.1 = 0 Then
While Ping.1 = 0 : Wend
Select Case Editzeile
Case 1
Decr Pfaktor
Case 2
Decr Ifaktor
Case 3
Decr Dfaktor
Case 4
Decr Mpfaktor
Case 5
Decr Mvfaktor
Case 6
Decr Mplfaktor
Case 7
Decr Mvlfaktor
End Select
End If
End If
If Pind.5 = 0 Then 'Zahl inkrementieren
Waitms 3
If Pind.5 = 0 Then
While Pind.5 = 0 : Wend
Select Case Editzeile
Case 1
Incr Pfaktor
Case 2
Incr Ifaktor
Case 3
Incr Dfaktor
Case 4
Incr Mpfaktor
Case 5
Incr Mvfaktor
Case 6
Incr Mplfaktor
Case 7
Incr Mvlfaktor
End Select
End If
End If
If Ping.0 = 0 Then 'Werte aus Display übernehmen Waitms 3 If Ping.0 = 0 Then While Ping.0 = 0 : Wend P_faktor = Pfaktor P_faktor = P_faktor / 10000
I_faktor = Ifaktor I_faktor = I_faktor / 1000000
D_faktor = Dfaktor D_faktor = D_faktor / 1000
Mp_faktor = Mpfaktor Mp_faktor = Mp_faktor / 10000
Mv_faktor = Mvfaktor Mv_faktor = Mv_faktor / 1000
Mpl_faktor = Mplfaktor Mpl_faktor = Mpl_faktor / 10000
Mvl_faktor = Mvlfaktor Mvl_faktor = Mvl_faktor / 1000
Sichern = 1 Call Lcd_cls Lcd_print Titel , 0 , 0 , 1 , 1 , 1 , Yellow , Blue Wait 2 End If End If
Text = " Regelparameter:" Text = Trimtext(text) Lcd_print Text , 0 , 18 , 1 , 1 , 1 , Black , White '************************************************** Text = Str(pfaktor) Text = " P = " + Text Text = Trimtext(text) If Editzeile = 1 Then Lcd_print Text , 0 , 36 , 1 , 1 , 1 , Black , Green '************************************************** Else Lcd_print Text , 0 , 36 , 1 , 1 , 1 , Black , White '************************************************** End If Text = Str(ifaktor) Text = " I = " + Text Text = Trimtext(text) If Editzeile = 2 Then Lcd_print Text , 0 , 46 , 1 , 1 , 1 , Black , Green '************************************************** Else Lcd_print Text , 0 , 46 , 1 , 1 , 1 , Black , White '************************************************** End If Text = Str(dfaktor) Text = " D = " + Text Text = Trimtext(text) If Editzeile = 3 Then Lcd_print Text , 0 , 56 , 1 , 1 , 1 , Black , Green '************************************************** Else Lcd_print Text , 0 , 56 , 1 , 1 , 1 , Black , White '************************************************** End If Text = Str(mpfaktor) Text = " MP = " + Text Text = Trimtext(text) If Editzeile = 4 Then Lcd_print Text , 0 , 66 , 1 , 1 , 1 , Black , Green '************************************************** Else Lcd_print Text , 0 , 66 , 1 , 1 , 1 , Black , White '************************************************** End If Text = Str(mvfaktor) Text = " MV = " + Text Text = Trimtext(text) If Editzeile = 5 Then Lcd_print Text , 0 , 76 , 1 , 1 , 1 , Black , Green '************************************************** Else Lcd_print Text , 0 , 76 , 1 , 1 , 1 , Black , White '************************************************** End If Text = Str(mplfaktor) Text = " MPL = " + Text Text = Trimtext(text) If Editzeile = 6 Then Lcd_print Text , 0 , 86 , 1 , 1 , 1 , Black , Green '************************************************** Else Lcd_print Text , 0 , 86 , 1 , 1 , 1 , Black , White '************************************************** End If Text = Str(mvlfaktor) Text = " MVL = " + Text Text = Trimtext(text) If Editzeile = 7 Then Lcd_print Text , 0 , 96 , 1 , 1 , 1 , Black , Green '************************************************** Else Lcd_print Text , 0 , 96 , 1 , 1 , 1 , Black , White '************************************************** End If If Transferok = 1 Then Lcd_print " Transfer OK" , 0 , 106 , 1 , 1 , 1 , Black , White Transferok = 0 End if Case 4 'siehe COM IN 'Siehe Datenempfang Case 5 'Logging 'If Porte.7 = 0 Then Activate_sd_bus 'If Antriebaus = 0 Then 'Motoren laufen ->Werte Loggen ' If Dateiaktuell = 0 Then 'Keine Datei geöffnet ' Lname = 1 ' Dateilaenge = 1 'Nächste Schleife einmal erzwingen ' While Dateilaenge > 0 ' Dateiname = Str(lname) ' Dateiname = Dateiname + ".log" ' '0 voranstellen, damit sich die Dateien besser sortieren lassen. ' While Len(dateiname) < 12 ' Dateiname = "0" + Dateiname ' Wend ' Dateilaenge = Filelen(dateiname) ' Incr Lname ' Wend ' If Porte.7 = 1 Then call Activate_display_bus ' Lcd_print Dateiname , 25 , 75 , 1 , 1 , 1 , Black , White ' If Porte.7 = 0 Then Activate_sd_bus ' Dateiaktuell = 1 ' Loopcount = 0 ' _sec = 0 ' _min = 0 ' _hour = 0 ' Zeit = "00:00:00" ' Open Dateiname For Append As #2 ' Logtext = "Zeit;Loopcount;U;I;Nick;GyroNick;Stellwert;Roll;Pl;Pr;Ksum;V" ' Print #2 , Logtext ' End If
' If Loopcount <> Aloopcount Then ' Ulog = U / 100 ' Ilog = I / 100 ' Kmhlog = Kmh / 10 ' Logtext = Zeit ' Logtext = Logtext + ";" ' Logtext = Logtext + Str(loopcount) ' Logtext = Logtext + ";" ' Logtext = Logtext + Fusing(ulog , "#.#") ' Logtext = Logtext + ";" ' Logtext = Logtext + Fusing(ilog , "#.#") ' Logtext = Logtext + ";" ' Logtext = Logtext + Str(mit_acc_y) ' Logtext = Logtext + ";" ' Logtext = Logtext + Str(gyro_nick) ' Logtext = Logtext + ";" ' Logtext = Logtext + Fusing(stellwert , "#.###") ' Logtext = Logtext + ";" ' Logtext = Logtext + Str(mit_acc_x) ' Logtext = Logtext + ";" ' Logtext = Logtext + Str(ml) ' Logtext = Logtext + ";" ' Logtext = Logtext + Str(mr) ' Logtext = Logtext + ";" ' Logtext = Logtext + Fusing(k_sum , "#.##") ' Logtext = Logtext + ";" ' Logtext = Logtext + Fusing(kmhlog , "#.#") ' Print #2 , Logtext ' Aloopcount = Loopcount ' End If 'Else ' Close #2 ' Dateiaktuell = 0 'End If
End Select Loop
'*************************************************************************** Datenempfang:
!PUSH R24;
!IN R24, SREG;
!PUSH R24;
!LDS R24,{EP1}
!STS {EP0},R24
!LDS R24,{EP2}
!STS {EP1},R24
!LDS R24,{EP3}
!STS {EP2},R24
!LDS R24,{EP4}
!STS {EP3},R24
!LDS R24,{EP5}
!STS {EP4},R24
!IN R24,UDR1
!STs {EP5},R24
!POP R24;
!Out Sreg , R24;
!POP R24;
'Debugging If Modus = 4 Then
Hexstr = Hex(ep0) Text = Hexstr + " " Hexstr = Hex(ep1) Text = Text + Hexstr + " " Hexstr = Hex(ep2) Text = Text + Hexstr + " " Hexstr = Hex(ep3) Text = Text + Hexstr + " " Hexstr = Hex(ep4) Text = Text + Hexstr + " " Hexstr = Hex(ep5) Text = Text + Hexstr + " " Lcd_print Text , 0 , 75 , 1 , 1 , 1 , Black , White
End If
'< Serielle Telegramme abarbeiten> If Ep0 = 13 Then
If Ep5 = 13 Then
!lds R24,{EP1}
!LDs R25,{EP2}
!add R24,R25
!LDs R25,{EP3}
!add R24,R25
!STS {Bdummy},R24
If Bdummy = Ep4 Then 'Die Prüfsumme stimmt
Select Case Index 'Telegrammnummer
Case 1 'Spannung
U = Iwert
Case 2 'Strom
I = Iwert
Case 3 'km/h
Kmh = Iwert
Case 4 'Ta und Antriebaus
Antriebaus = Ep2
Ta = Ep3
Case 5 'Akkuprozente
Akkuprozente = Iwert
Case 6 'Kilometer
Kilometer = Wwert
Case 7 'Meter
Meter = Iwert
Case 8 'Roll_l
Maccx1 = Ep2
Maccx2 = Ep3
Case 9 'Roll_h
Maccx3 = Ep2
Maccx4 = Ep3
Mit_acc_x = Maccx
Case 10 'Nick_l
Maccy1 = Ep2
Maccy2 = Ep3
Case 11 'Nick_h
Maccy3 = Ep2
Maccy4 = Ep3
Mit_acc_y = Maccy
Case 12 'Null_Roll_l
Nullx1 = Ep2
Nullx2 = Ep3
Case 13 'Null_Roll_h
Nullx3 = Ep2
Nullx4 = Ep3
Null_x = Nullx
Case 14 'Null_Nick_l
Nully1 = Ep2
Nully2 = Ep3
Case 15 'Null_Nick_h
Nully3 = Ep2
Nully4 = Ep3
Null_y = Nully
Case 16
Sw1 = Ep2
Sw2 = Ep3
Case 17
Sw3 = Ep2
Sw4 = Ep3
Stellwert = Sw
Case 18
Gyronick1 = Ep2
Gyronick2 = Ep3
Case 19
Gyronick3 = Ep2
Gyronick4 = Ep3
Gyro_nick = Gyronick
Case 20
Kksum1 = Ep2
Kksum2 = Ep3
Case 21
Kksum3 = Ep2
Kksum4 = Ep3
K_sum = Kksum
Case 255
If Sichern = 1 Then
!lds R24,{P_f1}
!LDs R25,{P_f2}
!add R24,R25
!LDs R25,{P_f3}
!add R24,R25
!LDs R25,{P_f4}
!add R24,R25
!LDs R25,{I_f1}
!add R24,R25
!LDs R25,{I_f2}
!add R24,R25
!LDs R25,{I_f3}
!add R24,R25
!LDs R25,{I_f4}
!add R24,R25
!LDs R25,{D_f1}
!add R24,R25
!LDs R25,{D_f2}
!add R24,R25
!LDs R25,{D_f3}
!add R24,R25
!LDs R25,{D_f4}
!add R24,R25
!LDs R25,{Mp_f1}
!add R24,R25
!LDs R25,{Mp_f2}
!add R24,R25
!LDs R25,{Mp_f3}
!add R24,R25
!LDs R25,{Mp_f4}
!add R24,R25
!LDs R25,{Mv_f1}
!add R24,R25
!LDs R25,{Mv_f2}
!add R24,R25
!LDs R25,{Mv_f3}
!add R24,R25
!LDs R25,{Mv_f4}
!add R24,R25
!LDs R25,{Mpl_f1}
!add R24,R25
!LDs R25,{Mpl_f2}
!add R24,R25
!LDs R25,{Mpl_f3}
!add R24,R25
!LDs R25,{Mpl_f4}
!add R24,R25
!LDs R25,{Mvl_f1}
!add R24,R25
!LDs R25,{Mvl_f2}
!add R24,R25
!LDs R25,{Mvl_f3}
!add R24,R25
!LDs R25,{Mvl_f4}
!add R24,R25
!STS {Bdummy},R24
'Open "COM2:" For Binary As #1 'SD-Karte
'Print #1 , Chr(p_f1) ; Chr(p_f2) ; Chr(p_f3) ; Chr(p_f4);
'Print #1 , Chr(i_f1) ; Chr(i_f2) ; Chr(i_f3) ; Chr(i_f4);
'Print #1 , Chr(d_f1) ; Chr(d_f2) ; Chr(d_f3) ; Chr(d_f4);
'Print #1 , Chr(mp_f1) ; Chr(mp_f2) ; Chr(mp_f3) ; Chr(mp_f4);
'Print #1 , Chr(mv_f1) ; Chr(mv_f2) ; Chr(mv_f3) ; Chr(mv_f4);
'Print #1 , Chr(mpl_f1) ; Chr(mpl_f2) ; Chr(mpl_f3) ; Chr(mpl_f4);
'Print #1 , Chr(mvl_f1) ; Chr(mvl_f2) ; Chr(mvl_f3) ; Chr(mvl_f4);
'Print #1 , Chr(bdummy) ; Chr(13);
'Close #1
Transferok = 1
Sichern = 0
End If
Case 76 'Linker Motor
Ml = Ep3
If Ep2 = 0 Then Ml = Ml * -1
Incr Loopcount
Case 82 'Rechter Motor
Mr = Ep3
If Ep2 = 0 Then Mr = Mr * -1
End Select
If Modus = 2 Then
Select Case Index
Case 36 'Port a und C Status
Hauptstportc = Ep2
Hauptstporta = Ep3
Case 37 'Fussschalter 1
Ad0 = Wwert
Case 38 'Fussschalter 2
Ad1 = Wwert
Case 39 'Fussschalter gesamt
Fussschalter = Ep2
End Select
End If
If Modus < 3 Then
Select Case Index
Case 22
P_f1 = Ep2
P_f2 = Ep3
Case 23
P_f3 = Ep2
P_f4 = Ep3
Case 24
I_f1 = Ep2
I_f2 = Ep3
Case 25
I_f3 = Ep2
I_f4 = Ep3
Case 26
D_f1 = Ep2
D_f2 = Ep3
Case 27
D_f3 = Ep2
D_f4 = Ep3
Case 28
Mp_f1 = Ep2
Mp_f2 = Ep3
Case 29
Mp_f3 = Ep2
Mp_f4 = Ep3
Case 30
Mv_f1 = Ep2
Mv_f2 = Ep3
Case 31
Mv_f3 = Ep2
Mv_f4 = Ep3
Case 32
Mpl_f1 = Ep2
Mpl_f2 = Ep3
Case 33
Mpl_f3 = Ep2
Mpl_f4 = Ep3
Case 34
Mvl_f1 = Ep2
Mvl_f2 = Ep3
Case 35
Mvl_f3 = Ep2
Mvl_f4 = Ep3
End Select
End If
End If 'Prüfsumme stimmt
End If
End If '</Serielle Telegramme abarbeiten> Return '******************************************************************************
Sub Activate_display_bus:
'remove SD Card from bus, must be "Reset PortF.7" for board D071x 'Reset Porte.7 'different to what the SD card needs 'Config Spi = Hard , Interrupt = Off , Data Order = Msb , Master = Yes , Polarity = Low , Phase = 0 , Clockrate = 4 , Noss = 1 'Set Spsr.spi2x 'hidden parameter: doubles the speed of the SPI output 'Spiinit
End Sub
Sub Activate_sd_bus
'Deactivate Display 'Set Lcd_port.lcd_cs 'different to what the display needs 'Config Spi = Hard , Interrupt = Off , Data Order = Msb , Master = Yes , _ ' Polarity = High , Phase = 1 , Clockrate = 4 , Noss = 1 'no double speed. You may remove this line to try out with your card. 'It may work. 'Reset Spsr.spi2x 'Spiinit 'connects SD Card to bus, must to be "Set PortF.7" for board D071x 'Set Porte.7
End Sub
Function Trimtext(t As String) As String
If Len(t) > 22 Then T = Left(t , 22) While Len(t) < 22 : T = T + " " : Wend
End Function
Sub Minmax
Umin = 10000 Umax = -10000 Imin = 0 Imax = 0 Kmhmin = 0 Kmhmax = 0 Mlmax = 0 Mlmin = 0 Mrmax = 0 Mrmin = 0 Tamax = 0
End Sub
Tick:
Incr _sec If _sec > 59 Then _sec = 0 Incr _min End If
If _min > 59 Then _min = 0 Incr _hour End If
Zeit = Time$
Return
Getdatetime: Return
Sub Drawpagestatics
Call Lcd_print(titel , 0 , 0 , 1 , 1 , 1 , Yellow , Blue) Select Case Modus Case 0 Lcd_rect 8 , 84 , 124 , 95 , 0 , Black 'Rahmen für Akku Balken Lcd_rect 24 , 125 , 124 , 135 , 0 , Black 'Rahmen für Horizontal Lenker Lcd_rect 8 , 100 , 19 , 160 , 0 , Black 'Rahmen für Vertikal Lenker Lcd_draw 20 , 130 , 22 , 130 , 0 , Black 'Vertikaler Balken, Mitteltick Lcd_draw 74 , 136 , 74 , 138 , 0 , Black 'Horizontaler Balken, Mitteltick Lcd_draw 0 , 167 , 132 , 167 , 0 , Black 'Hintergrund der Fußzeile Case 4 Call Lcd_print( "COM-LOG" , 30 , 10 , 2 , 1 , 2 , Black , White) Text = " 0 1 2 3 4 5" Lcd_print Text , 0 , 60 , 1 , 1 , 1 , Black , White Case 5 Call Lcd_print( "LOGGING" , 30 , 40 , 2 , 1 , 2 , Black , White) 'Dateiaktuell = 0 End Select
End Sub
End
$include "Y:\Programme\Tools\BASCOM-AVR\Display 3000\Displaymodul\Bascom Programme\für Include\GLCD21_display3000.bas" $include "Y:\Programme\Tools\BASCOM-AVR\Display 3000\Displaymodul\Bascom Programme\für Include\GLCD21_fonts.bas"
Colortable:
Data 0
Sicherheitshinweis
Achtung! Gehen Sie so ein Projekt nur an, wenn Sie sehr gute Kenntisse in Mechanik und Elektronik haben.
Funktion
Kippt das Fahrzeug nach vorn, wird nach vorn beschleunigt und umgekehrt. Die Regelung erledigt ein PID-Regler. Danach kommen jedoch noch einige zusätzliche Faktoren die z.B. die Motorkennlinie ausgleichen sowie ungewollte Richtungsänderungen bei unebenen Oberflächen reduzieren. Ähnlich aufwändig wird es bei den Motorreglern. Eine reine Blockkommutierung, wie bei solchen Motoren üblich, ist auf unebenen Untergründen suboptimal. Hier wurde zusätzliche Hallsensoren installiert.
Die seitliche Stabilität garantiert der Kreiseleffekt den jeder vom Fahrrad kennt. Das endgültige Fahrzeug wird eine Lenkung bekommen, die auf dem Beharrungsmoment basiert. Eine konventionelle Lenkung ist nicht möglich weil es nur ein Rad gibt.
Elektronik
Motorcontroller
Der Motorcontroller regelt den Motor, misst und übermittelt der Motorregler auch Spannung, Strom und Fahrstrecke. Die Temperatur des Kühlkörpers wird ebenfalls gemessen und wenn nötig ein Lüfter zugeschaltet. Die Einheit wird vom PC aus über eine ISP Schnittstelle programmiert.
Der Motorregler ist auf einer doppelseitigen Platine aufgebaut. Die Verbindung zwischen den beiden Platinen bildet ein 10-adriges Flachkabel. Zusätzlich zu den beiden Platinen gehört zum Motorregler auch die im Motor verbaute Sensorplatine. Auf dieser werden die Signale der drei zusätzlichen Hallsensoren zu einem Signal zusammengefasst.
Hauptcontroller
Die Hauptcontroller erledigt die Lageregelung. Sie steuert die Motoren und gibt die entsprechenden Meldungen an den Anzeigecontroller aus. Sie stellt auch die 12V für den Tacho bereit. Bei beiden Controllern wird ein Chopperspannungsregler eingesetzt der bis zu 63V verträgt. Deshalb ist eine Versorgung aus dem 24..48V Netz möglich. Die Lageregelung speichert auch die gefahrenen Km, einigen Min-Max Werte und diverse Werte bei einem Notaus. Diese Werte werden im EEprom als Text abgelegt. Beachten Sie, daß Sie das Gerät dann nur noch auf Privatgelände mit Zustimmung des Eigentümers benutzen dürfen. Eine Bertriebserlaubnis für das Gerät existiert nicht. Eine Einzelabnahme kann beim TÜV beantragt werden.
Schaltbild
- HauptSteuerung I2C Prototyp Rev0.pdf
- MotorSteuerung Prototyp Rev0 Blatt1.pdf
- MotorSteuerung Prototyp Rev0 Blatt2.pdf
- Hallgeber D Prototyp.pdf
- Listrik01-Verdrahtung Prototyp.pdf
- Listrik01 Prototyp Parts Rev0.pdf
Neigungssensoradapter
Im Unterschied zu ähnlichen Projekten besitzt dieses Fahrzeug einen über den I2C Bus gesteuerten Gyro und Accelerometer. Zum Beschleunigen und Bremsen werden die Sensoren gekippt.
Wie geht es weiter
Ich werde diese Seite updaten, je nach Fortschritt. Habe die nächsten paar Monate wenig Zeit....
