Aus RN-Wissen.de
Wechseln zu: Navigation, Suche
Laderegler Test Tueftler Seite

(Hauptsteuerung)
(Hauptsteuerung)
Zeile 938: Zeile 938:
  
 
Zur Hauptsteuerung gehört auch der Sensoradapter. (Schaltbild siehe Hauptsteuerung) Dieser stellt die 3,3V für die Sensoren bereit. Im Unterschied zu ähnlichen Projekten besitzt das Rutscherle2 kein Potentiometer für die Lenkung. Zum Lenken werden die gesamten Sensoren gekippt. Darum auch dieser Sensoradapter. Die Sensoren werden mit dem Adapter direkt mit der Achse der Lenkung verbunden. Die ideale Position ist dabei natürlich genau im Schnittpunkt von Lenk- und Motorachse.
 
Zur Hauptsteuerung gehört auch der Sensoradapter. (Schaltbild siehe Hauptsteuerung) Dieser stellt die 3,3V für die Sensoren bereit. Im Unterschied zu ähnlichen Projekten besitzt das Rutscherle2 kein Potentiometer für die Lenkung. Zum Lenken werden die gesamten Sensoren gekippt. Darum auch dieser Sensoradapter. Die Sensoren werden mit dem Adapter direkt mit der Achse der Lenkung verbunden. Die ideale Position ist dabei natürlich genau im Schnittpunkt von Lenk- und Motorachse.
 +
 +
===Software Version 66===
 +
 +
<pre>
 +
$prog &HFF , &HFF , &HD1 , &HFF                                                'Fusebits für Atmega644
 +
$regfile = "m644def.dat"
 +
$framesize = 64
 +
$swstack = 64
 +
$hwstack = 64
 +
$crystal = 20000000
 +
$baud = 57600
 +
Declare Sub Onbutton1
 +
Declare Sub Logtext(byval Adresse As Word , Text As String )
 +
Declare Sub Akkugrenzen
 +
Declare Sub Unfalldatenschreiber
 +
Declare Sub Betriebsdatenschreiber
 +
Config Portb.0 = Output
 +
Config Portb.1 = Output
 +
Config Portb.2 = Output
 +
Config Portb.3 = Output
 +
Config Portb.4 = Output
 +
Config Portb.6 = Input                                                          'Fusstaster
 +
Config Portc.0 = Input
 +
Config Portc.1 = Input
 +
Config Portc.2 = Input
 +
Config Portc.3 = Input
 +
Config Portc.4 = Input
 +
Config Portc.5 = Input
 +
Config Portc.6 = Input
 +
Config Portc.7 = Input
 +
Config Portd.7 = Output
 +
Config Portd.6 = Output
 +
Config Portd.5 = Output
 +
Config Portd.4 = Output
 +
Config Portd.3 = Output
 +
Config Portd.2 = Input
 +
Led1 Alias Portb.4
 +
Led2 Alias Portb.3
 +
Sensor_aus Alias Portb.2
 +
Sensor_test Alias Portb.1                                                      'Self-Test. 0=Normalbetrieb
 +
Led3 Alias Portd.4
 +
Led4 Alias Portd.5
 +
Led5 Alias Portd.6
 +
Led6 Alias Portd.3
 +
Led7 Alias Portd.7
 +
Button1 Alias Pind.2
 +
Fusstaster Alias Pinb.6
 +
'******************************************************************************************************************************
 +
'******************************************************************************************************************************
 +
'Konstanten für die Geschwindigkeits und Streckenmessung
 +
Const Kom_pro_u = 276                                                          '276*Kommutieren für eine Umdrehung.
 +
Const Radumpfang = 139                                                          'RaduMPFang in Zentimeter
 +
'Maximalwert bis zu dem die Abweichung 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 Vmax = 60                                                                'Vmax Vorwärts in km/h*10
 +
Const Vmaxr = -60                                                              'Vmax Rückwärts in km/h*10
 +
'Konstanten für die Akkus
 +
Const Akkuleer = 3600                                                          '36V, 12V pro Akku -> Akkus leer
 +
Const Akkufastleer = 3750
 +
'Konstanten für die Signalaufbereitoung
 +
Const Gyro_anz = 10000
 +
Const Acc_anz = 50
 +
Const Gyro_faktor = 91
 +
'******************************************************************************************************************************
 +
'******************************************************************************************************************************
 +
'Variablen für die Rückmeldungen von den Motorreglern
 +
Dim U As Integer At $100
 +
Dim I As Integer At $102
 +
Dim K As Integer At $104
 +
Dim Checksumme As Byte At $106
 +
Dim Ende As Byte At $107
 +
Dim Motormeldung(8) As Byte At $100 Overlay
 +
Dim M1 As Byte At $100 Overlay
 +
Dim M2 As Byte At $101 Overlay
 +
Dim M3 As Byte At $102 Overlay
 +
Dim M4 As Byte At $103 Overlay
 +
Dim M5 As Byte At $104 Overlay
 +
Dim M6 As Byte At $105 Overlay
 +
Dim M7 As Byte At $106 Overlay
 +
Dim M8 As Byte At $107 Overlay
 +
'Variablen für die Telegramme an die Motorregler
 +
Dim Checksum_rechts As Byte At $108
 +
Dim Checksum_links As Byte At $109
 +
Dim Richtung_rechts As Byte At $10a
 +
Dim Richtung_links As Byte At $10b
 +
Dim Pwm_rechts As Byte At $10c
 +
Dim Pwm_links As Byte At $10d
 +
Dim Zeiger_meldung As Byte At $10e
 +
Dim Ta As Byte At $10f
 +
Dim U_links As Integer At $111
 +
Dim U_rechts As Integer At $113
 +
Dim I_links As Integer At $115
 +
Dim I_rechts As Integer At $117
 +
Dim K_links As Integer At $119
 +
Dim K_rechts As Integer At $11b
 +
Dim B(5) As Byte At $11d
 +
Dim Kilometer As Word At $122
 +
Dim Kilometermeldung(2) As Byte At $122 Overlay
 +
Dim Akkustand As Integer At $124
 +
Dim Akkumeldung(2) As Byte At $124 Overlay
 +
Dim Kmh As Integer At $126
 +
Dim Kmhmeldung(2) As Byte At $126 Overlay
 +
Dim Meter As Integer At $128
 +
Dim Metermeldung(2) As Byte At $128 Overlay
 +
Dim Di As Single At $12a                                                        'Distanz pro Kommutieren
 +
Dim Kmhfaktor As Single At $12e
 +
Dim Zentimeter As Single At $132
 +
Dim Strecke As Single At $136                                                  'Gefahrene Strecke während 1/100s
 +
Dim Kx As Single At $13a                                                        'Anzahl Kommutierungen für die Streckenberechnung
 +
Dim Kv As Single At $13e                                                        'Anzahl Kommutierungen für die Geschwindigkeitosberechnung
 +
Dim Vbrems As Long At $142                                                      'Bremse bei Überlast oder zu hoher Geschwindigkeit
 +
Dim Lbrems As Long At $146
 +
Dim Brems As Long At $14a
 +
Dim P_faktor As Single At $14e
 +
Dim Pf1 As Byte At $14e Overlay
 +
Dim Pf2 As Byte At $14f Overlay
 +
Dim Pf3 As Byte At $150 Overlay
 +
Dim Pf4 As Byte At $151 Overlay
 +
Dim I_faktor As Single At $152
 +
Dim If1 As Byte At $152 Overlay
 +
Dim If2 As Byte At $153 Overlay
 +
Dim If3 As Byte At $154 Overlay
 +
Dim If4 As Byte At $155 Overlay
 +
Dim D_faktor As Single At $156
 +
Dim Df1 As Byte At $156 Overlay
 +
Dim Df2 As Byte At $157 Overlay
 +
Dim Df3 As Byte At $158 Overlay
 +
Dim Df4 As Byte At $159 Overlay
 +
Dim Gyro_nick As Word At $015a
 +
Dim Gyro_roll As Word At $015c
 +
Dim Gyro_roll_alt As Word At $015e
 +
Dim Gyro_nick_alt As Word At $0160
 +
Dim acc_nick As Long At $0162
 +
Dim Acc_roll As Long At $0166
 +
Dim Sum_acc_roll As Long At $016a
 +
Dim Sum_acc_nick As Long At $016e
 +
Dim Sum_gyro_roll As Long At $0172
 +
Dim Sum_gyro_nick As Long At $0176
 +
Dim Korr_acc_roll As Long At $017a
 +
Dim Korr_acc_nick As Long At $017e
 +
Dim Maccroll As Long At $0182
 +
Dim Maccroll1 As Byte At $0182 Overlay
 +
Dim Maccroll2 As Byte At $0183 Overlay
 +
Dim Maccroll3 As Byte At $0184 Overlay
 +
Dim Maccroll4 As Byte At $0185 Overlay
 +
Dim Maccnick As Long At $0186
 +
Dim maccnick1 As Byte At $0186 Overlay
 +
Dim maccnick2 As Byte At $0187 Overlay
 +
Dim maccnick3 As Byte At $0188 Overlay
 +
Dim maccnick4 As Byte At $0189 Overlay
 +
Dim Mit_acc_nick_s As Long At $018a
 +
Dim Mit_gyro_roll As Long At $018e
 +
Dim Mit_gyro_nick As Long At $0192
 +
Dim Sig_gyro_roll As Long At $0196
 +
Dim Sig_gyro_nick As Long At $019a
 +
 +
Dim Nullroll As Long At $019e
 +
Dim Nullroll1 As Byte At $019e Overlay
 +
Dim Nullroll2 As Byte At $019f Overlay
 +
Dim Nullroll3 As Byte At $01a0 Overlay
 +
Dim Nullroll4 As Byte At $01a1 Overlay
 +
Dim Nullnick As Long At $01a2
 +
Dim nullnick1 As Byte At $01a2 Overlay
 +
Dim nullnick2 As Byte At $01a3 Overlay
 +
Dim nullnick3 As Byte At $01a4 Overlay
 +
Dim nullnick4 As Byte At $01a5 Overlay
 +
Dim Stellwert As Single At $01a6
 +
Dim Mrg As Single At $01aa
 +
Dim Mrg1 As Byte At $01aa Overlay
 +
Dim Mrg2 As Byte At $01ab Overlay
 +
Dim Mrg3 As Byte At $01ac Overlay
 +
Dim Mrg4 As Byte At $01ad Overlay
 +
Dim Mra As Single At $01ae
 +
Dim Mra1 As Byte At $01ae Overlay
 +
Dim Mra2 As Byte At $01af Overlay
 +
Dim Mra3 As Byte At $01b0 Overlay
 +
Dim Mra4 As Byte At $01b1 Overlay
 +
Dim Kksum As Single At $01b2
 +
Dim Kksum1 As Byte At $01b2 Overlay
 +
Dim Kksum2 As Byte At $01b3 Overlay
 +
Dim Kksum3 As Byte At $01b4 Overlay
 +
Dim Kksum4 As Byte At $01b5 Overlay
 +
Dim Sw As Single At $01b6
 +
Dim Sw1 As Byte At $01b6 Overlay
 +
Dim Sw2 As Byte At $01b7 Overlay
 +
Dim Sw3 As Byte At $01b8 Overlay
 +
Dim Sw4 As Byte At $01b9 Overlay
 +
Dim Gyronick As Long At $01ba
 +
Dim Gyronick1 As Byte At $01ba Overlay
 +
Dim Gyronick2 As Byte At $01bb Overlay
 +
Dim Gyronick3 As Byte At $01bc Overlay
 +
Dim Gyronick4 As Byte At $01bd Overlay
 +
Dim Mp_faktor As Single At $01be
 +
Dim Mpf1 As Byte At $01be Overlay
 +
Dim Mpf2 As Byte At $01bf Overlay
 +
Dim Mpf3 As Byte At $01c0 Overlay
 +
Dim Mpf4 As Byte At $01c1 Overlay
 +
Dim Mv_faktor As Single At $01c2
 +
Dim Mvf1 As Byte At $01c2 Overlay
 +
Dim Mvf2 As Byte At $01c3 Overlay
 +
Dim Mvf3 As Byte At $01c4 Overlay
 +
Dim Mvf4 As Byte At $01c5 Overlay
 +
Dim Mpl_faktor As Single At $01c6
 +
Dim Mplf1 As Byte At $01c6 Overlay
 +
Dim Mplf2 As Byte At $01c7 Overlay
 +
Dim Mplf3 As Byte At $01c8 Overlay
 +
Dim Mplf4 As Byte At $01c9 Overlay
 +
Dim Mvl_faktor As Single At $01ca
 +
Dim Mvlf1 As Byte At $01ca Overlay
 +
Dim Mvlf2 As Byte At $01cb Overlay
 +
Dim Mvlf3 As Byte At $01cc Overlay
 +
Dim Mvlf4 As Byte At $01cd Overlay
 +
Dim P_tacho As Single At $01ce
 +
Dim I_tacho As Single At $01d2
 +
Dim D_tacho As Single At $01d6
 +
Dim Mp_tacho As Single At $01da
 +
Dim Mv_tacho As Single At $01de
 +
Dim Mpl_tacho As Single At $01e2
 +
Dim Mvl_tacho As Single At $01e6
 +
Dim Tdummy As Integer At $01ea
 +
Dim Tachomeldung(30) As Byte At $01ce Overlay
 +
Dim T1 As Byte At $01ce Overlay
 +
Dim T2 As Byte At $01cf Overlay
 +
Dim T3 As Byte At $01d0 Overlay
 +
Dim T4 As Byte At $01d1 Overlay
 +
Dim T5 As Byte At $01d2 Overlay
 +
Dim T6 As Byte At $01d3 Overlay
 +
Dim T7 As Byte At $01d4 Overlay
 +
Dim T8 As Byte At $01d5 Overlay
 +
Dim T9 As Byte At $01d6 Overlay
 +
Dim T10 As Byte At $01d7 Overlay
 +
Dim T11 As Byte At $01d8 Overlay
 +
Dim T12 As Byte At $01d9 Overlay
 +
Dim T13 As Byte At $01da Overlay
 +
Dim T14 As Byte At $01db Overlay
 +
Dim T15 As Byte At $01dc Overlay
 +
Dim T16 As Byte At $01dd Overlay
 +
Dim T17 As Byte At $01de Overlay
 +
Dim T18 As Byte At $01df Overlay
 +
Dim T19 As Byte At $01e0 Overlay
 +
Dim T20 As Byte At $01e1 Overlay
 +
Dim T21 As Byte At $01e2 Overlay
 +
Dim T22 As Byte At $01e3 Overlay
 +
Dim T23 As Byte At $01e4 Overlay
 +
Dim T24 As Byte At $01e5 Overlay
 +
Dim T25 As Byte At $01e6 Overlay
 +
Dim T26 As Byte At $01e7 Overlay
 +
Dim T27 As Byte At $01e8 Overlay
 +
Dim T28 As Byte At $01e9 Overlay
 +
Dim T29 As Byte At $01ea Overlay
 +
Dim T30 As Byte At $01eb Overlay
 +
Dim Werteneu As Byte
 +
Dim Mit_acc_roll As Long
 +
Dim Mit_acc_nick As Long
 +
Dim Null_roll As Long
 +
Dim Null_nick As Long
 +
Dim Mit_regelzeit_ges As Single
 +
Dim Lagefehler As Long
 +
Dim Regelzeit As Word                                                          'Die Zeit in 1/100s die gebraucht wird bis die Nullage erreicht ist
 +
Dim Regelzeitmax As Word
 +
Dim Mit_acc_roll_max As Long
 +
Dim Mit_acc_nick_max As Long
 +
Dim Mit_acc_roll_min As Long
 +
Dim Mit_acc_nick_min As Long
 +
Dim Pwm_gesamt As Single
 +
Dim Pwm_rechts_max As Byte
 +
Dim Pwm_links_max As Byte
 +
Dim Intcount As Byte
 +
Dim K_diff As Single
 +
Dim K_sum As Single
 +
Dim K_diff_min As Single
 +
Dim K_diff_max As Single
 +
Dim Pwm_lenk_rechts As Integer
 +
Dim Pwm_lenk_links As Integer
 +
Dim Lenkvorgabe As Single
 +
Dim Mp_anpassung As Single
 +
Dim Mv_anpassung As Single
 +
Dim Mpl_anpassung As Single
 +
Dim Mvl_anpassung As Single
 +
Dim K_sum_anpassung As Single
 +
Dim Wdummy1 As Word
 +
Dim Ldummyi As Long
 +
Dim Ldummy1 As Long
 +
Dim Ldummy2 As Long
 +
Dim Bdummy1 As Byte
 +
Dim Idummy1 As Integer
 +
Dim Sdummy1 As Single
 +
Dim Antriebaus As Byte
 +
Dim Notaus As Byte
 +
Dim Sensor_ok As Byte                                                          'Zeigt an ob die Mittelwerte der Gyros stabil stehen.
 +
Dim Sensor_ok_count As Byte
 +
Dim Ltest As Byte
 +
Dim Ugemessen As Long                                                          'Batteriespannung in V*100
 +
Dim Igemessen As Long                                                          'Gesamter Motorstrom in mA
 +
Dim Imin As Long                                                                'Kleinster gemessener Motorstrom (größte Rückspeisung)
 +
Dim Imax As Long                                                                'größter gemessener Motorstrom
 +
Dim Summe_as_entladen As Long
 +
Dim Summe_mah_entladen As Long
 +
Dim Summe_as_laden As Long
 +
Dim Summe_mah_laden As Long
 +
Dim Akkukap As Long
 +
Dim Akku1 As Long
 +
Dim Akku2 As Long
 +
Dim Akku3 As Long
 +
Dim Akku4 As Long
 +
Dim Akku5 As Long
 +
Dim Erzeugt As Long
 +
Dim Verbraucht As Long
 +
'Single Variablen um die Prozente rechnen zu können
 +
Dim Akkuinhalt As Single
 +
Dim Akkuverbrauch As Single
 +
Dim Akkuprozent As Single
 +
Dim Adresse As Word
 +
Dim Bzeit As Long
 +
Dim Bstate As Byte
 +
Dim Frei As Byte
 +
Dim Timeout As Byte
 +
Dim S_err As Byte
 +
Dim Seriell_err_l As Word
 +
Dim Seriell_err_r As Word
 +
Dim Tamax As Byte
 +
Dim Fusscount As Word
 +
Dim Blinkcount As Byte
 +
Dim Lenkerstellung As Single
 +
Dim Lenkstellwert As Single
 +
Dim Lenkfehler As Single
 +
Dim N As Integer
 +
Dim Utext As String * 32                                                        'Texte für den Unfalldatenschreiber (als Text im eeprom)
 +
Dim Ugrund As String * 32
 +
Dim Text As String * 16
 +
Dim Min_v As Integer
 +
Dim Max_v As Integer
 +
Dim Tachocount As Byte
 +
Led3 = 1
 +
Led4 = 1
 +
Led5 = 1
 +
Led6 = 1
 +
Led7 = 1
 +
Wait 5                                                                          '5 Sekunden Warten
 +
Antriebaus = 1
 +
'Ein paar feste Werte vorab berechnen:
 +
Di = Radumpfang / Kom_pro_u
 +
Kmhfaktor = Di * 9                                                              'Eigentlich 3.6 aber bei der Mittelwertbildung entsteht der 4-Fache Wert. Darum 9, und es ergibt sich automateisch V in 0.1km/h
 +
'Unbenutzte Pins auf Masse legen
 +
Portb.7 = 0
 +
Portb.5 = 0
 +
'Input-Pins hochziehen
 +
Portc.0 = 1
 +
Portc.1 = 1
 +
Portc.2 = 1
 +
Portc.3 = 1
 +
Portc.4 = 1
 +
Portc.5 = 1
 +
Portc.6 = 1
 +
'Sensorboard schalten:
 +
Sensor_aus = 0
 +
Sensor_test = 0
 +
'Hochziehwiderstände der Knöpfe einschalten
 +
Button1 = 1
 +
Fusstaster = 1
 +
Config Adc = Single , Prescaler = Auto , Reference = Internal_2.56
 +
Start Adc
 +
'Wartezeit bis die Motorregler soweit sind
 +
Readeeprom Max_v , $2f0
 +
Readeeprom Min_v , $2f2
 +
Readeeprom Summe_as_entladen , $300                                            'Entnommene Wh und Ws aus EEPROM lesen
 +
Led3 = 0
 +
Waitms 150
 +
Readeeprom Summe_mah_entladen , $310
 +
Led4 = 0
 +
Waitms 150
 +
Readeeprom Summe_as_laden , $320
 +
Led5 = 0
 +
Waitms 150
 +
Readeeprom Summe_mah_laden , $330
 +
Led6 = 0
 +
Waitms 150
 +
Readeeprom Akkukap , $340
 +
Led7 = 0
 +
Waitms 150
 +
Readeeprom Kilometer , $350
 +
Readeeprom Meter , $360
 +
Readeeprom Zentimeter , $370
 +
'<Faktoren für den PID-Regler einlesen>
 +
      Readeeprom P_faktor , $380
 +
      Readeeprom I_faktor , $384
 +
      Readeeprom D_faktor , $388
 +
      Readeeprom Mp_faktor , $38c
 +
      Readeeprom Mv_faktor , $390
 +
      Readeeprom Mpl_faktor , $394
 +
      Readeeprom Mvl_faktor , $398
 +
      If P_faktor < 0 Or P_faktor > 1 Then P_faktor = 0.0033                    'Halbwegs sinnvolle Werte vorbelegen
 +
      If I_faktor < 0 Or I_faktor > 1 Then I_faktor = 0.00006
 +
      If D_faktor < 0 Or D_faktor > 1 Then D_faktor = 0.08
 +
      If Mp_faktor < 0 Or Mp_faktor > 1 Then Mp_faktor = 0.006
 +
      If Mv_faktor < 0 Or Mv_faktor > 1 Then Mv_faktor = 0
 +
      If Mpl_faktor < 0 Or Mpl_faktor > 1 Then Mpl_faktor = 0
 +
      If Mvl_faktor < 0 Or Mvl_faktor > 1 Then Mvl_faktor = 0
 +
'</Faktoren für den PID-Regler einlesen>
 +
If Kilometer = $ffff Then                                                      'Kommt bei frischem EEPROM vor
 +
Zentimeter = 0
 +
Meter = 0
 +
Kilometer = 0
 +
Min_v = 0
 +
Max_v = 0
 +
End If
 +
'Bei Fehlerhaften Werten aus dem EEPROM einfach 100mAh als Vorgabe
 +
If Akkukap <= 0 Then Akkukap = 100
 +
If Akkukap > 32000 Then Akkukap = 100
 +
Akkugrenzen
 +
Antriebaus = 1                                                                  'Beim Start ist immer alles aus
 +
Notaus = 0
 +
Readeeprom Max_v , $2f0
 +
Readeeprom Min_v , $2f4
 +
Config Watchdog = 128
 +
'<Sicherheitstest 1>
 +
      Start Watchdog
 +
      If Button1 = 0 Then Wait 10
 +
      If Antriebaus = 0 Then Wait 10
 +
      If P_faktor > 1 Then Wait 10
 +
      If I_faktor > 1 Then Wait 10
 +
      If D_faktor > 1 Then Wait 10
 +
      If P_faktor < 0 Then Wait 10
 +
      If I_faktor < 0 Then Wait 10
 +
      If D_faktor < 0 Then Wait 10
 +
      If Fusstaster = 0 Then Wait 10
 +
      Stop Watchdog
 +
'</Sicherheitstest 1>
 +
'Maximalwerte für Datenschreiber/Notauserkennung auf 0 setzen
 +
Regelzeit = 0
 +
Regelzeitmax = 0
 +
Mit_acc_nick_max = 0
 +
Mit_acc_roll_max = 0
 +
Pwm_rechts_max = 0
 +
Pwm_links_max = 0
 +
K_diff_min = 0
 +
K_diff_max = 0
 +
Richtung_links = 2                                                              'Leerlauf
 +
Richtung_rechts = 2                                                            'Leerlauf
 +
Pwm_links = 0
 +
Pwm_rechts = 0
 +
Pwm_gesamt = 0
 +
Mit_acc_nick_s = 0
 +
Lenkerstellung = 0
 +
K_rechts = 0
 +
K_links = 0
 +
Seriell_err_l = 0
 +
Seriell_err_r = 0
 +
Ugrund = "----------------"                                                    'Einfach mal vorbelegen um zu sehen obs auch einen Notaus ohne Grund gibt.
 +
On Timer0 Regel_interrupt
 +
Config Timer0 = Timer , Prescale = 1024
 +
Enable Timer0
 +
Enable Interrupts
 +
Waitms 100
 +
Toggle Led3
 +
Toggle Led4
 +
Toggle Led5
 +
Toggle Led6
 +
Toggle Led7
 +
Waitms 100
 +
Toggle Led3
 +
Toggle Led4
 +
Toggle Led5
 +
Toggle Led6
 +
Toggle Led7
 +
Waitms 100
 +
Toggle Led3
 +
Toggle Led4
 +
Toggle Led5
 +
Toggle Led6
 +
Toggle Led7
 +
Waitms 100
 +
Toggle Led3
 +
Toggle Led4
 +
Toggle Led5
 +
Toggle Led6
 +
Toggle Led7
 +
'<Sicherheitstest 2>
 +
      Start Watchdog
 +
      If Button1 = 0 Then Wait 10
 +
      If Antriebaus = 0 Then Wait 10
 +
      If P_faktor > 1 Then Wait 10
 +
      If I_faktor > 1 Then Wait 10
 +
      If D_faktor > 1 Then Wait 10
 +
      If P_faktor < 0 Then Wait 10
 +
      If I_faktor < 0 Then Wait 10
 +
      If D_faktor < 0 Then Wait 10
 +
      If Ugemessen < 1000 Then Wait 10
 +
      If Ugemessen > 4700 Then Wait 10
 +
      If Fusstaster = 0 Then Wait 10
 +
      Stop Watchdog
 +
'</Sicherheitstest 2>
 +
If Ugemessen < 2400 Then                                                        'Sensoren ausschalten wenn am Ladegerät oder zu Testzwecken
 +
    Sensor_aus = 1
 +
Else
 +
    Sensor_aus = 0
 +
End If
 +
While Ugemessen < 2400                                                          'Unterspannung oder am Ladegerät!
 +
      Led3 = 1
 +
      Led4 = 1
 +
      Led5 = 1
 +
      Led6 = 0
 +
      Led7 = 1
 +
Wend
 +
'Prüfung ob Akku gerade geladen:
 +
If Ugemessen > 3950 Then                                                        'Der Akku wurde gerade geladen
 +
  Disable Interrupts                                                          'Interruts abschalten damit die folgenden Zeilen nicht gerade jetzt vom Interrupt unterbrochen werden.
 +
  Waitms 50
 +
  Summe_as_entladen = 0
 +
  Summe_mah_entladen = 0
 +
  Summe_as_laden = 0
 +
  Summe_mah_laden = 0
 +
  Enable Interrupts
 +
End If
 +
Summen_vorbelegen:
 +
acc_roll = Getadc(5)                                                              'Roll
 +
acc_nick = Getadc(7)                                                              'Nick
 +
Gyro_roll = Getadc(2)                                                          'Roll
 +
Gyro_nick = Getadc(4)                                                          'Nick
 +
acc_roll = acc_roll * 100
 +
acc_nick = acc_nick * 100
 +
Sum_gyro_roll = Gyro_roll * Gyro_anz
 +
Sum_gyro_nick = Gyro_nick * Gyro_anz
 +
Gyro_roll_alt = Gyro_roll
 +
Gyro_nick_alt = Gyro_nick
 +
Mit_gyro_nick = Gyro_roll
 +
Mit_gyro_nick = Gyro_nick
 +
Sum_acc_roll = acc_roll * Acc_anz
 +
Sum_acc_nick = acc_nick * Acc_anz
 +
Waitms 100
 +
Toggle Led3
 +
Toggle Led4
 +
Toggle Led5
 +
Toggle Led6
 +
Toggle Led7
 +
Waitms 100
 +
Toggle Led3
 +
Toggle Led4
 +
Toggle Led5
 +
Toggle Led6
 +
Toggle Led7
 +
If Sig_gyro_nick < -3 Then Goto Summen_vorbelegen
 +
If Sig_gyro_nick > 3 Then Goto Summen_vorbelegen
 +
If Sig_gyro_roll < -3 Then Goto Summen_vorbelegen
 +
If Sig_gyro_roll > 3 Then Goto Summen_vorbelegen
 +
'<Sicherheitstest 3>
 +
      Start Watchdog
 +
      If Button1 = 0 Then Wait 10
 +
      If Antriebaus = 0 Then Wait 10
 +
      If P_faktor > 1 Then Wait 10
 +
      If I_faktor > 1 Then Wait 10
 +
      If D_faktor > 1 Then Wait 10
 +
      If P_faktor < 0 Then Wait 10
 +
      If I_faktor < 0 Then Wait 10
 +
      If D_faktor < 0 Then Wait 10
 +
      If Ugemessen < 1000 Then Wait 10
 +
      If Ugemessen > 4700 Then Wait 10
 +
      If Fusstaster = 0 Then Wait 10
 +
      Stop Watchdog
 +
'</Sicherheitstest 3>
 +
'********************************************************************************************************************************************************
 +
Do
 +
'<Fusstaster abfragen>
 +
If Fusscount = 0 Then
 +
  If Antriebaus = 0 Then
 +
      Antriebaus = 1
 +
      Waitms 100                                                                'Damit der Regelinterrupt auch was davon mitbekommt
 +
      Disable Interrupts
 +
      Betriebsdatenschreiber
 +
      Enable Interrupts
 +
  End If
 +
End If
 +
'</Fusstaster abfragen>
 +
'<Sicherheitstest Sensoren>
 +
      If Antriebaus = 1 Then
 +
        If Intcount <> Ltest Then
 +
            Ltest = Intcount
 +
            Sensor_ok = 0
 +
            If Sensor_ok_count > 0 Then Decr Sensor_ok_count
 +
            '1. Es darf keine nennenswerte Drehbewegung erkannt werden!
 +
            If Sig_gyro_nick < -3 Then Sensor_ok_count = 50
 +
            If Sig_gyro_nick > 3 Then Sensor_ok_count = 50
 +
            If Sig_gyro_roll < -3 Then Sensor_ok_count = 50
 +
            If Sig_gyro_roll > 3 Then Sensor_ok_count = 50
 +
            '2. Die Werte dürfen nicht verrauscht sein
 +
            Ldummy2 = Mit_acc_nick + 500
 +
            If acc_nick > Ldummy2 Then Sensor_ok_count = 50
 +
            Ldummy2 = Mit_acc_nick - 500
 +
            If acc_nick < Ldummy2 Then Sensor_ok_count = 50
 +
            Ldummy2 = Mit_acc_roll + 500
 +
            If acc_roll > Ldummy2 Then Sensor_ok_count = 50
 +
            Ldummy2 = Mit_acc_roll - 500
 +
            If acc_roll < Ldummy2 Then Sensor_ok_count = 50
 +
            Ldummy2 = Mit_gyro_nick + 5
 +
            If Gyro_nick > Ldummy2 Then Sensor_ok_count = 50
 +
            Ldummy2 = Mit_gyro_nick - 5
 +
            If Gyro_nick < Ldummy2 Then Sensor_ok_count = 50
 +
            Ldummy2 = Mit_gyro_roll + 5
 +
            If Gyro_roll > Ldummy2 Then Sensor_ok_count = 50
 +
            Ldummy2 = Mit_gyro_roll - 5
 +
            If Gyro_roll < Ldummy2 Then Sensor_ok_count = 50
 +
            '3. Die Werte müssen ungefähr in einer legalen Nullage sein.
 +
            If Null_roll < 66150 Then Sensor_ok_count = 50
 +
            If Null_roll > 69150 Then Sensor_ok_count = 50
 +
            If null_nick < 63850 Then Sensor_ok_count = 50
 +
            If null_nick > 66850 Then Sensor_ok_count = 50
 +
            If Sensor_ok_count = 0 Then Sensor_ok = 1                          'Bedingungen während 50 Messungen (0.5s) eingehalten
 +
        End If
 +
      Else
 +
        Sensor_ok = 0
 +
      End If
 +
'</Sicherheitstest Sensoren>
 +
'<Batterieanzeige>
 +
      If Ugemessen > Akkufastleer Then                                          'alles unter 36V Leerlaufspannung ist LEER
 +
              If Verbraucht > Akkukap Then
 +
                  Akkukap = Verbraucht + 1                                      'Einfach mal annehmen daß noch mehr als angezeigt im Akku ist
 +
                  Akkugrenzen
 +
              End If
 +
      End If
 +
      'Mit Blinken wenn Gyros nicht bereit
 +
      If Antriebaus = 1 Then
 +
      If Ugemessen > Akkuleer Then                                              'alles unter 36V Leerlaufspannung ist LEER
 +
              If Verbraucht > Akkukap Then
 +
                  Akkukap = Verbraucht + 20                                    'Einfach mal annehmen daß noch mehr als angezeigt im Akku ist
 +
                  Akkugrenzen
 +
              End If
 +
      End If
 +
        If Sensor_ok = 1 Then
 +
            If Verbraucht < Akku1 Then Led7 = 0 Else Led7 = 1
 +
            If Verbraucht < Akku2 Then Led6 = 0 Else Led6 = 1
 +
            If Verbraucht < Akku3 Then Led5 = 0 Else Led5 = 1
 +
            If Verbraucht < Akku4 Then Led4 = 0 Else Led4 = 1
 +
            If Verbraucht < Akku5 Then Led3 = 0 Else Led3 = 1
 +
        Else
 +
        If Frei = 1 Then                                                      'Schneller Blinken wenn Frei
 +
            Blinkcount = Intcount
 +
            If Blinkcount > 50 Then Blinkcount = Blinkcount - 50
 +
        Else
 +
            Blinkcount = Intcount / 2
 +
        End If
 +
 +
            If Blinkcount > 40 Then
 +
              If Verbraucht < Akku1 Then Led7 = 0 Else Led7 = 1
 +
              If Verbraucht < Akku2 Then Led6 = 0 Else Led6 = 1
 +
              If Verbraucht < Akku3 Then Led5 = 0 Else Led5 = 1
 +
              If Verbraucht < Akku4 Then Led4 = 0 Else Led4 = 1
 +
              If Verbraucht < Akku5 Then Led3 = 0 Else Led3 = 1
 +
            Else
 +
              Led7 = 1
 +
              Led6 = 1
 +
              Led5 = 1
 +
              Led4 = 1
 +
              Led3 = 1
 +
            End If
 +
        End If
 +
      Else
 +
        If Verbraucht < Akku1 Then Led7 = 0 Else Led7 = 1
 +
        If Verbraucht < Akku2 Then Led6 = 0 Else Led6 = 1
 +
        If Verbraucht < Akku3 Then Led5 = 0 Else Led5 = 1
 +
        If Verbraucht < Akku4 Then Led4 = 0 Else Led4 = 1
 +
        If Verbraucht < Akku5 Then Led3 = 0 Else Led3 = 1
 +
      End If
 +
'</Batterieanzeige>
 +
'<Maximalwerte für Betriebsdatenschreiber>
 +
      If Igemessen < Imin Then Imin = Igemessen                                'Nur zur Information
 +
      If Igemessen > Imax Then Imax = Igemessen                                'Nur zur Information
 +
      If Regelzeit > Regelzeitmax Then Regelzeitmax = Regelzeit
 +
      If Mit_acc_nick > Mit_acc_nick_max Then Mit_acc_nick_max = Mit_acc_nick
 +
      If Mit_acc_roll > Mit_acc_roll_max Then Mit_acc_roll_max = Mit_acc_roll
 +
      If Mit_acc_nick < Mit_acc_nick_min Then Mit_acc_nick_min = Mit_acc_nick
 +
      If Mit_acc_roll < Mit_acc_roll_min Then Mit_acc_roll_min = Mit_acc_roll
 +
      If Pwm_links > Pwm_links_max Then Pwm_links_max = Pwm_links
 +
      If Pwm_rechts > Pwm_rechts_max Then Pwm_rechts_max = Pwm_rechts
 +
      If K_diff > K_diff_max Then K_diff_max = K_diff
 +
      If K_diff < K_diff_min Then K_diff_min = K_diff
 +
      If Ta > Tamax Then Tamax = Ta
 +
'</Maximalwerte für Betriebsdatenschreiber>
 +
If Intcount > 80 Then Led1 = 1 Else Led1 = 0                                    'Sekundenblinken
 +
                                                                                'Daran lässt sich leicht erkennen ob
 +
                                                                                'noch alles normal läuft.
 +
Debounce Button1 , 0 , Onbutton1 , Sub
 +
Loop
 +
End
 +
'**************************************************************************************************
 +
'Ab hier kommen die Subroutinen und die Interrupts
 +
'**************************************************************************************************
 +
Sub Akkugrenzen
 +
      'Grenzwerte für die Akku-LEDs berechnen
 +
      Ldummy1 = Akkukap / 5
 +
      Akku1 = Ldummy1
 +
      Akku2 = 2 * Ldummy1
 +
      Akku3 = 3 * Ldummy1
 +
      Akku4 = 4 * Ldummy1
 +
      Akku5 = 5 * Ldummy1
 +
End Sub
 +
'**************************************************************************************************
 +
Sub Unfalldatenschreiber
 +
Waitms 10
 +
  Utext = Str(regelzeit)
 +
  Utext = "Rz=" + Utext
 +
  Logtext 0 , Utext
 +
  Utext = Str(mit_acc_nick_max)
 +
  Utext = "N max=" + Utext
 +
  Logtext $10 , Utext
 +
  Utext = Str(mit_acc_nick)
 +
  Utext = "N=" + Utext
 +
  Logtext $20 , Utext
 +
  Utext = Str(mit_acc_nick_min)
 +
  Utext = "N min=" + Utext
 +
  Logtext $30 , Utext
 +
  Utext = Str(mit_acc_roll_max)
 +
  Utext = "R max=" + Utext
 +
  Logtext $40 , Utext
 +
  Utext = Str(mit_acc_roll)
 +
  Utext = "R=" + Utext
 +
  Logtext $50 , Utext
 +
  Utext = Str(mit_acc_roll_min)
 +
  Utext = "R min=" + Utext
 +
  Logtext $60 , Utext
 +
  Utext = Str(pwm_rechts)
 +
  Utext = "PWM r=" + Utext
 +
  Logtext $70 , Utext
 +
  Utext = Str(pwm_links)
 +
  Utext = "PWM l=" + Utext
 +
  Logtext $80 , Utext
 +
  Utext = Str(u_rechts)
 +
  Utext = "U r=" + Utext
 +
  Logtext $90 , Utext
 +
  Utext = Str(u_links)
 +
  Utext = "U l=" + Utext
 +
  Logtext $a0 , Utext
 +
  Utext = Str(ta)
 +
  Utext = "Ta=" + Utext
 +
  Logtext $b0 , Utext
 +
  Utext = Str(seriell_err_l)
 +
  Utext = Utext + "|"
 +
  Utext = Utext + Str(seriell_err_r)
 +
  Utext = "S =" + Utext
 +
  Logtext $c0 , Utext
 +
  Utext = Ugrund
 +
  Logtext $d0 , Utext
 +
End Sub
 +
'**************************************************************************************************
 +
Sub Betriebsdatenschreiber
 +
'Ob die Wartezeiten beim EEprom schreiben nötig sind ist nicht sicher.
 +
'Mit den Wartezeiten treten aber keine Fehler bei den Werten mehr auf.
 +
'Möglich, daß dies auch durch eine andere Änderung bewirkt wurde.
 +
Writeeeprom Max_v , $2f0
 +
Writeeeprom Min_v , $2f2
 +
Zentimeter = Int(zentimeter)                                                    'Die Millimeter sollen nicht geschrieben werden
 +
Writeeeprom Summe_as_entladen , $300
 +
Writeeeprom Summe_mah_entladen , $310
 +
Writeeeprom Summe_as_laden , $320
 +
Writeeeprom Summe_mah_laden , $330
 +
Writeeeprom Akkukap , $340
 +
Writeeeprom Kilometer , $350
 +
Writeeeprom Meter , $360
 +
Writeeeprom Zentimeter , $370
 +
  If Werteneu = 1 Then                                                        'Nur wenn das Einstellklavier gesteckt ist.
 +
      Writeeeprom P_faktor , $380
 +
      Writeeeprom I_faktor , $384
 +
      Writeeeprom D_faktor , $388
 +
      Writeeeprom Mp_faktor , $38c
 +
      Writeeeprom Mv_faktor , $390
 +
      Writeeeprom Mpl_faktor , $394
 +
      Writeeeprom Mvl_faktor , $398
 +
      Werteneu = 0
 +
  End If
 +
  Utext = Str(pwm_rechts_max)
 +
  Utext = "PWM r max=" + Utext
 +
  Logtext $e0 , Utext
 +
  Utext = Str(pwm_links_max)
 +
  Utext = "PWM l max=" + Utext
 +
  Logtext $f0 , Utext
 +
  Utext = Str(ugemessen)
 +
  Utext = "U=" + Utext
 +
  Logtext $100 , Utext
 +
  Utext = Str(imin)
 +
  Utext = "Imin=" + Utext
 +
  Logtext $110 , Utext
 +
  Utext = Str(imax)
 +
  Utext = "Imax=" + Utext
 +
  Logtext $120 , Utext
 +
  Utext = Str(summe_mah_entladen)
 +
  Utext = "-mAh" + Utext
 +
  Logtext $130 , Utext
 +
  Utext = Str(summe_mah_laden)
 +
  Utext = "+mAh=" + Utext
 +
  Logtext $140 , Utext
 +
  Utext = Str(verbraucht)
 +
  Utext = "    =" + Utext
 +
  Logtext $150 , Utext
 +
  Utext = Str(akkukap)
 +
  Utext = "Kap=" + Utext
 +
  Logtext $160 , Utext
 +
  Utext = Str(kilometer)
 +
  Utext = "Km=" + Utext
 +
  Logtext $170 , Utext
 +
  Utext = Str(meter)
 +
  Utext = "M=" + Utext
 +
  Logtext $180 , Utext
 +
  Utext = Str(zentimeter)
 +
  Utext = "cm=" + Utext
 +
  Logtext $190 , Utext
 +
  Utext = Str(regelzeitmax)
 +
  Utext = "Rz Max=" + Utext
 +
  Logtext $1a0 , Utext
 +
  Utext = Str(Null_roll)
 +
  Utext = "Null_R=" + Utext
 +
  Logtext $1b0 , Utext
 +
  Utext = Str(null_nick)
 +
  Utext = "null_N=" + Utext
 +
  Logtext $1c0 , Utext
 +
  Utext = Str(p_faktor)
 +
  Utext = "P=" + Utext
 +
  Logtext $1d0 , Utext
 +
  Utext = Str(i_faktor)
 +
  Utext = "I=" + Utext
 +
  Logtext $1e0 , Utext
 +
  Utext = Str(d_faktor)
 +
  Utext = "D=" + Utext
 +
  Logtext $1f0 , Utext
 +
  Utext = Str(mp_faktor)
 +
  Utext = "M=" + Utext
 +
  Logtext $200 , Utext
 +
  Utext = Str(k_diff_min)
 +
  Utext = "K min=" + Utext
 +
  Logtext $210 , Utext
 +
  Utext = Str(k_diff_max)
 +
  Utext = "K max=" + Utext
 +
  Logtext $220 , Utext
 +
  Utext = Str(seriell_err_l)
 +
  Utext = "S l=" + Utext
 +
  Logtext $230 , Utext
 +
  Utext = Str(seriell_err_r)
 +
  Utext = "S r=" + Utext
 +
  Logtext $240 , Utext
 +
  Utext = Str(tamax)
 +
  Utext = "Ta_Max=" + Utext
 +
  Logtext $250 , Utext
 +
  Utext = Str(max_v)
 +
  Utext = "V_Max=" + Utext
 +
  Logtext $260 , Utext
 +
  Utext = Str(min_v)
 +
  Utext = "V_Min=" + Utext
 +
  Logtext $270 , Utext
 +
End Sub
 +
'**************************************************************************************************
 +
Sub Onbutton1
 +
If Antriebaus = 1 Then
 +
  If Ugemessen > Akkuleer Then                                                'alles unter 36V Leerlaufspannung ist LEER
 +
      If Sensor_ok = 1 Then                                                    'Prüfen ob Gyros noch driften
 +
        Disable Interrupts                                                    'Interupts abschalten damit Fusscount nicht gerade jetzt decrementiert wird
 +
        Regelzeit = 0
 +
        Regelzeitmax = 0
 +
        Mit_acc_nick_max = 0
 +
        Mit_acc_roll_max = 0
 +
        Mit_acc_nick_min = 0
 +
        Mit_acc_roll_min = 0
 +
        Pwm_rechts_max = 0
 +
        Pwm_links_max = 0
 +
        Seriell_err_r = 0
 +
        Seriell_err_l = 0
 +
        K_diff_min = 0
 +
        K_diff_max = 0
 +
        Antriebaus = 0
 +
        Fusscount = 50                                                        '0.5 Sekunden zeit zum Aufsteigen
 +
        Enable Interrupts
 +
      Else
 +
        If null_nick > 66850 And null_nick < 67450 Then
 +
            If Bzeit = 500 Then
 +
              Bstate = 1
 +
              Bzeit = 0
 +
            Else
 +
              If Bstate = 1 Then
 +
                  If Bzeit < 50 Then
 +
                  Disable Interrupts
 +
                  Utext = "                "
 +
                  Logtext $00 , Utext
 +
                  Logtext $10 , Utext
 +
                  Logtext $20 , Utext
 +
                  Logtext $30 , Utext
 +
                  Logtext $40 , Utext
 +
                  Logtext $50 , Utext
 +
                  Logtext $60 , Utext
 +
                  Logtext $70 , Utext
 +
                  Logtext $80 , Utext
 +
                  Logtext $90 , Utext
 +
                  Logtext $a0 , Utext
 +
                  Logtext $b0 , Utext
 +
                  Logtext $c0 , Utext
 +
                  Logtext $d0 , Utext
 +
                  Logtext $e0 , Utext
 +
                  Logtext $f0 , Utext
 +
                  Logtext $100 , Utext
 +
                  Logtext $110 , Utext
 +
                  Logtext $120 , Utext
 +
                  Logtext $130 , Utext
 +
                  Logtext $140 , Utext
 +
                  Logtext $150 , Utext
 +
                  Logtext $160 , Utext
 +
                  Logtext $170 , Utext
 +
                  Logtext $180 , Utext
 +
                  Logtext $190 , Utext
 +
                  Logtext $1a0 , Utext
 +
                  Logtext $1b0 , Utext
 +
                  Logtext $1c0 , Utext
 +
                  Logtext $1d0 , Utext
 +
                  Logtext $1e0 , Utext
 +
                  Logtext $1f0 , Utext
 +
                  Logtext $200 , Utext
 +
                  Logtext $210 , Utext
 +
                  Logtext $220 , Utext
 +
                  Logtext $230 , Utext
 +
                  Logtext $240 , Utext
 +
                  Logtext $250 , Utext
 +
                  Logtext $260 , Utext
 +
                  Summe_as_entladen = 0
 +
                  Summe_mah_entladen = 0
 +
                  Summe_as_laden = 0
 +
                  Summe_mah_laden = 0
 +
                  Writeeeprom Summe_as_entladen , $300
 +
                  Writeeeprom Summe_mah_entladen , $310
 +
                  Writeeeprom Summe_as_laden , $320
 +
                  Writeeeprom Summe_mah_laden , $330
 +
                  Frei = 0
 +
                  For Bzeit = 1 To 10
 +
                    Led3 = 1
 +
                    Led4 = 1
 +
                    Led5 = 1
 +
                    Led6 = 1
 +
                    Led7 = 1
 +
                    Waitms 100
 +
                    Led3 = 0
 +
                    Led4 = 0
 +
                    Led5 = 0
 +
                    Led6 = 0
 +
                    Led7 = 0
 +
                    Waitms 100
 +
                  Next
 +
                  Bzeit = 500
 +
                  Enable Interrupts
 +
                  Waitms 100
 +
 +
                  End If
 +
                  If Bzeit > 250 And Bzeit < 350 Then
 +
                    Frei = 1
 +
                    Bzeit = 500
 +
                  Else
 +
                    Frei = 0
 +
                    Bzeit = 500
 +
 +
                  End If
 +
              End If
 +
            End If
 +
        Else
 +
        Frei = 0
 +
        Bzeit = 500
 +
        End If
 +
      End If
 +
  Else
 +
      If Verbraucht < Akkukap Then                                              'Akku ist laut Spannung leer!
 +
        Akkukap = Verbraucht * 0.95                                            'Einfach mal annehmen daß zuviel entnommen wurde
 +
        Akkugrenzen
 +
      End If
 +
      Disable Interrupts
 +
      Betriebsdatenschreiber
 +
      Enable Interrupts
 +
  End If
 +
Else
 +
  Antriebaus = 1
 +
  Waitms 100
 +
  Frei = 0
 +
  Disable Interrupts
 +
  Betriebsdatenschreiber
 +
  Enable Interrupts
 +
End If
 +
End Sub
 +
'**************************************************************************************************
 +
Sub Logtext(adresse , Text)
 +
  If Len(text) > 16 Then Text = Mid(text , 1 , 16)
 +
  While Len(text) < 16
 +
      Text = Text + " "
 +
  Wend
 +
  Writeeeprom Text , Adresse
 +
 +
End Sub
 +
'**************************************************************************************************
 +
Regel_interrupt:
 +
'**************************************************************************************************
 +
'Eigentlich ist das kein Typischer Interrupt. Der Interrupt wird hier nur verwendet um der
 +
'Regelung einen 100Hz Takt zu geben. Normalerweise sollte kein Interrupt so lange sein.
 +
'Dadurch werden andere Interrupts für diese Dauer blockiert.
 +
'**************************************************************************************************
 +
Timer0 = 61                                                                    'Sollte ca. 100Hz ergeben
 +
Led2 = 1
 +
If Fusscount > 0 Then Decr Fusscount
 +
If Fusstaster = 0 Then Fusscount = 25                                          '0.25 Sekunden ohne Fusstaster
 +
Bdummy1 = Udr
 +
acc_roll = Getadc(5)                                                              'Roll
 +
acc_nick = Getadc(7)                                                              'Nick
 +
Gyro_roll = Getadc(2)                                                          'Roll
 +
Gyro_nick = Getadc(4)                                                          'Nick
 +
acc_roll = acc_roll * 100
 +
acc_nick = acc_nick * 100
 +
'<Mittellage ausgleichen>
 +
  'Die Offsetwerte werden langsam an eine früher ermittelte Waagerechte Positoion angeglichen.
 +
  Null_roll = Null_roll * 99
 +
  null_nick = null_nick * 99
 +
  Null_roll = Null_roll + Offset_roll                                                'Mittellage
 +
  null_nick = null_nick + Offset_nick                                                'Mittellage
 +
  Null_roll = Null_roll / 100
 +
  null_nick = null_nick / 100
 +
'</Mittellage ausgleichen>
 +
'Offsetwerte der Beschleunigungssensoren sofort abziehen
 +
'Diese werden nicht laufend ermittelt
 +
acc_roll = acc_roll - Null_roll
 +
acc_nick = acc_nick - null_nick
 +
'<Geschwindikgeitsbegrenzung>
 +
      'Bei der Geschwindigkeitsbegrenzung wird einfach die Nullage der Plattform verändert.
 +
      'Beim Vorwärtsbremsen wird nach hinten geneigt.
 +
      'Beim Rückwärtsbremsen nach vorne.
 +
      'Höchstgeschwindigkeit im Gesetzlichen Rahmen unter 6Km/h (Ausserhalb der KFZ Zulassungsordnung)
 +
      Vbrems = 0
 +
      Lbrems = 0
 +
      If Frei = 0 Then
 +
        If Kmh > Vmax Then Vbrems = Vmax - Kmh
 +
        If Kmh < Vmaxr Then Vbrems = Vmaxr - Kmh
 +
      End If
 +
      'Lastabhängige Bremse
 +
      'Es muss immer genug Reserve sein um noch zu Bremsen oder zu Balancieren
 +
      Ldummyi = Int(pwm_gesamt)
 +
      If Ldummyi > Spwm Then Lbrems = Spwm - Ldummyi
 +
      If Ldummyi < Spwmr Then Lbrems = Spwmr - Ldummyi
 +
      Brems = Lbrems + Vbrems
 +
      Brems = Brems * 50
 +
      'Nicht zu heftig Bremsen
 +
      If Brems < -2000 Then Brems = -2000
 +
      If Brems > 2000 Then Brems = 2000
 +
      acc_nick = acc_nick + Brems
 +
'</Geschwindikgeitsbegrenzung>
 +
'<Signalaufbereitung der Sensorsignale>
 +
      K_diff = K_rechts - K_links
 +
      K_sum = K_sum * 0.84
 +
      K_sum = K_sum + K_diff
 +
      'Gleitender Mittelwert der Gyros berechnen um damit den Offset bei Stillstand
 +
      'zu haben. Dieser ist leider Temperaturabhängig.
 +
      Sum_gyro_roll = Sum_gyro_roll - Mit_gyro_roll
 +
      Sum_gyro_roll = Sum_gyro_roll + Gyro_roll
 +
      Mit_gyro_roll = Sum_gyro_roll / Gyro_anz
 +
      Sig_gyro_roll = Gyro_roll - Mit_gyro_roll
 +
      Korr_acc_roll = Sig_gyro_roll * Gyro_faktor
 +
      Sum_acc_roll = Sum_acc_roll - Mit_acc_roll
 +
      Sum_acc_roll = Sum_acc_roll + acc_roll
 +
      Sum_acc_roll = Sum_acc_roll + Korr_acc_roll                                        'Gyrowert hinzu
 +
      Mit_acc_roll = Sum_acc_roll / Acc_anz
 +
'************************************************************************************************************************************************************************************
 +
      Sdummy1 = Mit_acc_roll * K_sum                                            'Das soll ausgleichen, daß die Gyros bei gekippten Sensoren eine Drehung um die falsche Achse
 +
                                                                                'registrieren.
 +
      Sdummy1 = Sdummy1 / 2000                                                  'Die Sensorplatine muss dazu absolut Waagerecht montiert sein.
 +
      Lagefehler = Int(sdummy1)
 +
      Gyro_nick = Gyro_nick - Lagefehler                                        'Das soll den Fehler ausgleichen, der durch die geänderte Lage des Gyros auftritt.
 +
'************************************************************************************************************************************************************************************
 +
      Sum_gyro_nick = Sum_gyro_nick - Mit_gyro_nick
 +
      Sum_gyro_nick = Sum_gyro_nick + Gyro_nick
 +
      Mit_gyro_nick = Sum_gyro_nick / Gyro_anz
 +
      Sig_gyro_nick = Gyro_nick - Mit_gyro_nick
 +
      Korr_acc_nick = Sig_gyro_nick * Gyro_faktor
 +
      Sum_acc_nick = Sum_acc_nick - Mit_acc_nick
 +
      Sum_acc_nick = Sum_acc_nick + acc_nick
 +
      Sum_acc_nick = Sum_acc_nick - Korr_acc_nick                              'Gyrowert hinzu
 +
      Mit_acc_nick = Sum_acc_nick / Acc_anz
 +
'</Signalaufbereitung der Sensorsignale>
 +
'<PID-Regler>
 +
  Stellwert = 0
 +
  '<P-Regler>
 +
      Sdummy1 = Mit_acc_nick
 +
      Sdummy1 = Sdummy1 * P_faktor
 +
      Stellwert = Stellwert - Sdummy1
 +
  '</P-Regler>
 +
  '<I-Regler>
 +
      Mit_acc_nick_s = Mit_acc_nick_s + Mit_acc_nick
 +
      'Überlauf/unsinnige Werte verhindern
 +
      If Mit_acc_nick_s > Max_mit_acc_nick_s Then Mit_acc_nick_s = Max_mit_acc_nick_s
 +
      If Mit_acc_nick_s < Min_mit_acc_nick_s Then Mit_acc_nick_s = Min_mit_acc_nick_s
 +
      Sdummy1 = Mit_acc_nick_s
 +
      Sdummy1 = Sdummy1 * I_faktor
 +
      Stellwert = Stellwert - Sdummy1
 +
  '</I-Regler>
 +
  '<D-Regeler>
 +
      Sdummy1 = Sig_gyro_nick
 +
      Sdummy1 = Sdummy1 * D_faktor
 +
      Stellwert = Stellwert + Sdummy1
 +
  '</D-Regler>
 +
  '<Anpassungsfaktoren berechnen>
 +
  K_sum_anpassung = K_rechts + K_links
 +
  K_sum_anpassung = Abs(k_sum_anpassung)
 +
  Mp_anpassung = Pwm_gesamt * Mp_faktor
 +
  Mp_anpassung = Abs(mp_anpassung)
 +
  Mp_anpassung = Mp_anpassung + 0.95
 +
  Mv_anpassung = K_sum_anpassung * Mv_faktor
 +
  Mv_anpassung = Abs(mv_anpassung)
 +
  Mv_anpassung = Mv_anpassung + 1
 +
  Mpl_anpassung = Pwm_gesamt * Mpl_faktor
 +
  Mpl_anpassung = Abs(mpl_anpassung)
 +
  Mpl_anpassung = Mpl_anpassung + 1
 +
  Mvl_anpassung = K_sum_anpassung * Mvl_faktor
 +
  Mvl_anpassung = Abs(mvl_anpassung)
 +
  Mvl_anpassung = Mvl_anpassung + 1
 +
  '</Anpassungsfaktoren berechnen>
 +
  '<Verstärkte Regelung bei hoher Leistung>
 +
  Stellwert = Stellwert * Mp_anpassung
 +
  Stellwert = Stellwert * Mv_anpassung
 +
  '<Verstärkte Regelung bei hoher Leistung>
 +
  '<Abgeschwächte Regelung in Stand>
 +
  If K_sum_anpassung < 8 Then Stellwert = Stellwert * 0.8
 +
  If K_sum_anpassung < 6 Then Stellwert = Stellwert * 0.8
 +
  If K_sum_anpassung < 4 Then Stellwert = Stellwert * 0.8
 +
  '</Abgeschwächte Regelung in Stand>
 +
  Pwm_gesamt = Pwm_gesamt + Stellwert
 +
'</PID-Regler>
 +
'<Bremsvibrator einmischen>
 +
  'Der Bremsvibrator signalisiert dem Fahrer, daß wegen irgend einem Grenzwert gebremst wird.
 +
  'Das kann V-Max oder P-Max sein.
 +
    If Brems <> 0 Then
 +
      Bdummy1 = Intcount / 2
 +
      Bdummy1 = Bdummy1 Mod 2
 +
      If Bdummy1 = 0 Then
 +
        Pwm_gesamt = Pwm_gesamt + 5
 +
      Else
 +
        Pwm_gesamt = Pwm_gesamt - 5
 +
      End If
 +
    End If
 +
'<Bremsvibrator einmischen>
 +
'<Lenkung>
 +
  Lenkvorgabe = Mit_acc_roll / 80
 +
  Sdummy1 = Abs(lenkvorgabe)
 +
  If Sdummy1 <= 6 Then
 +
      Sdummy1 = Sdummy1 / 12
 +
      Lenkerstellung = Lenkvorgabe * Sdummy1
 +
  Else
 +
      If Lenkvorgabe < 0 Then
 +
        Lenkerstellung = Lenkvorgabe + 3
 +
      Else
 +
        Lenkerstellung = Lenkvorgabe - 3
 +
      End If
 +
  End If
 +
  'Lenkerstellung = Lenkerstellung * MP_anpassung                            'Dabei kann sich die Lenkung extrem aufschaukeln.
 +
  'Lenkfehler berechnen und zum Stellwert hinzu.
 +
  Lenkfehler = Lenkerstellung + K_sum
 +
  Lenkfehler = Lenkfehler * Mpl_anpassung
 +
  Lenkfehler = Lenkfehler * Mvl_anpassung
 +
  Lenkstellwert = Lenkerstellung + Lenkfehler
 +
  If Lenkstellwert > Max_lenk Then Lenkstellwert = Max_lenk
 +
  If Lenkstellwert < Min_lenk Then Lenkstellwert = Min_lenk
 +
  Pwm_lenk_links = Pwm_gesamt + Lenkstellwert
 +
  Pwm_lenk_rechts = Pwm_gesamt - Lenkstellwert
 +
  If Pwm_lenk_links > 0 Then Richtung_links = 1 Else Richtung_links = 0
 +
  If Pwm_lenk_rechts > 0 Then Richtung_rechts = 1 Else Richtung_rechts = 0
 +
  Pwm_lenk_links = Abs(pwm_lenk_links)
 +
  Pwm_lenk_rechts = Abs(pwm_lenk_rechts)
 +
  If Pwm_lenk_links > Maxpwm Then Pwm_lenk_links = Maxpwm
 +
  If Pwm_lenk_rechts > Maxpwm Then Pwm_lenk_rechts = Maxpwm
 +
  If Pwm_lenk_links < Minpwm Then Pwm_lenk_links = Minpwm
 +
  If Pwm_lenk_rechts < Minpwm Then Pwm_lenk_rechts = Minpwm
 +
  Pwm_links = Pwm_lenk_links
 +
  Pwm_rechts = Pwm_lenk_rechts
 +
'</Lenkung>
 +
If Antriebaus = 1 Then
 +
  If Notaus = 1 Then
 +
      'Unfalldatenschreiber: *********************************************************
 +
      'Dauert zwar Lange, macht aber nix. Bei Notaus muss nix geregelt werden.
 +
      Unfalldatenschreiber
 +
      Betriebsdatenschreiber
 +
      Notaus = 0
 +
      '*******************************************************************************
 +
  End If
 +
'Auf 0 setzen was sonst geregelt wird
 +
  Richtung_links = 2                                                          'Leerlauf
 +
  Richtung_rechts = 2                                                          'Leerlauf
 +
  Pwm_links = 0
 +
  Pwm_rechts = 0
 +
  Pwm_gesamt = 0
 +
  Mit_acc_nick_s = 0
 +
  Lenkerstellung = 0
 +
  K_rechts = 0
 +
  K_links = 0
 +
  'Waagerechte Positoion setzen wenn Zustand Antriebaus:
 +
  Wdummy1 = Getadc(5)
 +
  Null_roll = Wdummy1 * 100
 +
  Wdummy1 = Getadc(7)
 +
  null_nick = Wdummy1 * 100
 +
  'Mittelwert der Beschleunigungssensoren auf aktuellen Wert stellen
 +
  'Der Mittelwert braucht sonst zu lange bis
 +
  'er sich eingeregelt hat
 +
  'Sum_acc_roll = Acc_roll * Acc_anz
 +
  'Sum_acc_nick = Acc_nick * Acc_anz
 +
  If Intcount = 0 Then T30 = 1                                                'Regelparameter vom Tacho anfordern
 +
  If Intcount = 50 Then T30 = 1                                                'Regelparameter vom Tacho anfordern
 +
End If
 +
'*******************************************************************************
 +
  If T30 = 1 Then                                                              'Regelparameter vom Tacho anfordern
 +
      K_links = 0
 +
      K_rechts = 0
 +
      Zeiger_meldung = 1
 +
      Bdummy1 = Udr
 +
      T30 = 0                                                                  'Damit keine alte Rückmeldung ausgewertet wird
 +
      Timeout = Timer0
 +
      Timeout = Timeout + 100
 +
      Print Chr(255) ; Chr(0) ; Chr(0) ; Chr(255) ; Chr(13)
 +
      While Zeiger_meldung < 31
 +
        If Ucsr0a.rxc0 = 1 Then                                                'Warten bis ein Byte eingetroffen ist
 +
            Tachomeldung(zeiger_meldung) = Udr
 +
            Incr Zeiger_meldung
 +
            Timeout = Timer0
 +
            Timeout = Timeout + 20
 +
        End If
 +
        If Timer0 > Timeout Then
 +
        T30 = 0
 +
        Exit While                                                            'Timeout
 +
        End If
 +
      Wend
 +
      If T30 = 13 Then
 +
        $asm
 +
            lds R24,{t1}
 +
            LDs R25,{t2}
 +
            add R24,R25
 +
            LDs R25,{t3}
 +
            add R24,R25
 +
            LDs R25,{t4}
 +
            add R24,R25
 +
            LDs R25,{t5}
 +
            add R24,R25
 +
            LDs R25,{t6}
 +
            add R24,R25
 +
            LDs R25,{t7}
 +
            add R24,R25
 +
            LDs R25,{t8}
 +
            add R24,R25
 +
            LDs R25,{t9}
 +
            add R24,R25
 +
            LDs R25,{t10}
 +
            add R24,R25
 +
            LDs R25,{t11}
 +
            add R24,R25
 +
            LDs R25,{t12}
 +
            add R24,R25
 +
            LDs R25,{t13}
 +
            add R24,R25
 +
            LDs R25,{t14}
 +
            add R24,R25
 +
            LDs R25,{t15}
 +
            add R24,R25
 +
            LDs R25,{t16}
 +
            add R24,R25
 +
            LDs R25,{t17}
 +
            add R24,R25
 +
            LDs R25,{t18}
 +
            add R24,R25
 +
            LDs R25,{t19}
 +
            add R24,R25
 +
            LDs R25,{t20}
 +
            add R24,R25
 +
            LDs R25,{t21}
 +
            add R24,R25
 +
            LDs R25,{t22}
 +
            add R24,R25
 +
            LDs R25,{t23}
 +
            add R24,R25
 +
            LDs R25,{t24}
 +
            add R24,R25
 +
            LDs R25,{t25}
 +
            add R24,R25
 +
            LDs R25,{t26}
 +
            add R24,R25
 +
            LDs R25,{t27}
 +
            add R24,R25
 +
            LDs R25,{t28}
 +
            add R24,R25
 +
            STS {Bdummy1},R24
 +
        $end Asm
 +
 +
        If Bdummy1 = T29 Then
 +
            P_faktor = P_tacho
 +
            I_faktor = I_tacho
 +
            D_faktor = D_tacho
 +
            Mp_faktor = Mp_tacho
 +
            Mv_faktor = Mv_tacho
 +
            Mpl_faktor = Mpl_tacho
 +
            Mvl_faktor = Mvl_tacho
 +
            Werteneu = 1
 +
        End If
 +
      End If
 +
  Else
 +
  'Errechnete Geschwindigkeiten an Motoren Senden:
 +
  'Rechter Motor:
 +
  Checksum_rechts = 82
 +
  Checksum_rechts = Checksum_rechts + Richtung_rechts
 +
  Checksum_rechts = Checksum_rechts + Pwm_rechts
 +
  'Geschwindigkeit an Motorregler senden
 +
  Zeiger_meldung = 1
 +
  Bdummy1 = Udr                                                                'Puffer leeren
 +
  'Print Chr(82) ; Chr(richtung_rechts) ; Chr(pwm_rechts) ; Chr(checksum_rechts) ; Chr(13);
 +
  'In Assembler geht das so:
 +
  $asm
 +
  Ldi R24 , 82
 +
  STS    udr,R24
 +
  Warten1:
 +
  LDS    R24,UCSR0A
 +
  BST    R24,5
 +
  Brtc warten1
 +
  LDS    R24,{Richtung_rechts}
 +
  STS    udr,R24
 +
  Warten2:
 +
  LDS    R24,UCSR0A
 +
  BST    R24,5
 +
  Brtc warten2
 +
  lds    R24,{Pwm_rechts}
 +
  STS    udr,R24
 +
  Warten3:
 +
  LDS    R24,UCSR0A
 +
  BST    R24,5
 +
  Brtc warten3
 +
  LDS    R24,{Checksum_rechts}
 +
  STS    udr,R24
 +
  Warten4:
 +
  LDS    R24,UCSR0A
 +
  BST    R24,5
 +
  Brtc warten4
 +
  LDI    R24,13
 +
  STS    udr,R24
 +
  Warten5:
 +
  LDS    R24,UCSR0A
 +
  BST    R24,5
 +
  Brtc warten5
 +
  $end Asm
 +
  Motormeldung(8) = 0                                                          'Damit keine alte Rückmeldung ausgewertet wird
 +
  Timeout = Timer0
 +
  Timeout = Timeout + 80
 +
  While Zeiger_meldung < 9
 +
      If Ucsr0a.rxc0 = 1 Then                                                  'Warten bis ein Byte eingetroffen ist
 +
        Motormeldung(zeiger_meldung) = Udr
 +
        Incr Zeiger_meldung
 +
      End If
 +
      If Timer0 > Timeout Then
 +
      Ende = 0
 +
      Exit While                                                                'Timeout
 +
      End If
 +
  Wend
 +
  $asm
 +
              lds R24,{M1}
 +
              LDs R25,{M2}
 +
              ADD R24,R25
 +
              LDs R25,{M3}
 +
              ADD R24,R25
 +
              LDs R25,{M4}
 +
              ADD R24,R25
 +
              LDs R25,{M5}
 +
              ADD R24,R25
 +
              LDs R25,{M6}
 +
              ADD R24,R25
 +
              STS {Bdummy1},R24
 +
  $end Asm
 +
  If Bdummy1 <> Checksumme Then Ende = 0
 +
  If Ende = 13 Then
 +
      U_rechts = U
 +
      I_rechts = I
 +
      K_rechts = K
 +
  Else
 +
      Incr Seriell_err_r
 +
      S_err = 1
 +
  End If
 +
  'Linker Motor:
 +
  Checksum_links = 76
 +
  Checksum_links = Checksum_links + Richtung_links
 +
  Checksum_links = Checksum_links + Pwm_links
 +
  Zeiger_meldung = 1
 +
  Bdummy1 = Udr                                                                'Puffer leeren
 +
  'Print Chr(76) ; Chr(richtung_links) ; Chr(pwm_links) ; Chr(checksum_links) ; Chr(13);
 +
  'In Assembler geht das so:
 +
  $asm
 +
  Ldi    R24 , 76
 +
  STS    udr,R24
 +
  Warten6:
 +
  LDS    R24,UCSR0A
 +
  BST    R24,5
 +
  Brtc warten6
 +
  LDS    R24,{Richtung_links}
 +
  STS    udr,R24
 +
  Warten7:
 +
  LDS    R24,UCSR0A
 +
  BST    R24,5
 +
  Brtc warten7
 +
  lds    R24,{Pwm_links}
 +
  STS    udr,R24
 +
  Warten8:
 +
  LDS    R24,UCSR0A
 +
  BST    R24,5
 +
  Brtc warten8
 +
  LDS    R24,{Checksum_links}
 +
  STS    udr,R24
 +
  Warten9:
 +
  LDS    R24,UCSR0A
 +
  BST    R24,5
 +
  Brtc warten9
 +
  LDI    R24,13
 +
  STS    udr,R24
 +
  Warten10:
 +
  LDS    R24,UCSR0A
 +
  BST    R24,5
 +
  Brtc warten10
 +
  $end Asm
 +
  Motormeldung(8) = 0                                                          'Damit keine alte Rückmeldung ausgewertet wird
 +
  Timeout = Timer0
 +
  Timeout = Timeout + 80
 +
  While Zeiger_meldung < 9
 +
      If Ucsr0a.rxc0 = 1 Then                                                  'Warten bis ein Byte eingetroffen ist
 +
        Motormeldung(zeiger_meldung) = Udr
 +
        Incr Zeiger_meldung
 +
      End If
 +
      If Timer0 > Timeout Then
 +
      Ende = 0
 +
      Exit While                                                                'Timeout
 +
      End If
 +
  Wend
 +
  $asm
 +
              lds R24,{M1}
 +
              LDs R25,{M2}
 +
              ADD R24,R25
 +
              LDs R25,{M3}
 +
              ADD R24,R25
 +
              LDs R25,{M4}
 +
              ADD R24,R25
 +
              LDs R25,{M5}
 +
              ADD R24,R25
 +
              LDs R25,{M6}
 +
              ADD R24,R25
 +
              STS {Bdummy1},R24
 +
  $end Asm
 +
  If Bdummy1 <> Checksumme Then Ende = 0
 +
  If Ende = 13 Then
 +
      U_links = U
 +
      I_links = I
 +
      K_links = K
 +
  Else
 +
      Incr Seriell_err_l
 +
      S_err = 1
 +
  End If
 +
If S_err = 1 Then                                                              'Sicherheitshalber, damit der Lenkausgleich nicht überreagiert.
 +
  K_links = 0
 +
  K_rechts = 0
 +
  S_err = 0
 +
End If
 +
End If
 +
If Antriebaus = 0 Then
 +
'<Notauskriterien ermitteln>
 +
      If Mit_acc_nick > -300 Then
 +
        If Mit_acc_nick < 300 Then
 +
            Regelzeit = 0
 +
        End If
 +
      End If
 +
  Incr Regelzeit
 +
'</Notauskriterien ermitteln>
 +
'<Notauskriterien abprüfen>
 +
  If Regelzeit > 250 Then                                                      'Wenn nach 2,5s waagerechte Lage nicht erreicht
 +
      Notaus = 1
 +
      Antriebaus = 1
 +
      Ugrund = Str(regelzeit)
 +
      Ugrund = "*Rz " + Ugrund
 +
  End If
 +
  Ldummyi = 4000 + Brems
 +
  If Mit_acc_nick > Ldummyi Then                                                  'zu weit nach hinten gekippt
 +
      Notaus = 1
 +
      Antriebaus = 1
 +
      Ugrund = Str(mit_acc_nick)
 +
      Ugrund = "*Nick " + Ugrund
 +
  End If
 +
  Ldummyi = -4000 + Brems
 +
  If Mit_acc_nick < Ldummyi Then                                                  'zu weit nach vorn gekippt
 +
      Notaus = 1
 +
      Antriebaus = 1
 +
      Ugrund = Str(mit_acc_nick)
 +
      Ugrund = "*Nick " + Ugrund
 +
  End If
 +
  If Mit_acc_roll > 5000 Then                                                    'Lenker zu weit
 +
      Notaus = 1
 +
      Antriebaus = 1
 +
      Ugrund = Str(mit_acc_roll)
 +
      Ugrund = "*Roll " + Ugrund
 +
  End If
 +
  If Mit_acc_roll < -5000 Then                                                    'Lenker zu weit
 +
      Notaus = 1
 +
      Antriebaus = 1
 +
      Ugrund = Str(mit_acc_roll)
 +
      Ugrund = "*Roll " + Ugrund
 +
  End If
 +
  If K_diff > 10 Then                                                          'Plattform rotiert zu schnell
 +
      Notaus = 1
 +
      Antriebaus = 1
 +
      Ugrund = Str(k_diff)
 +
      Ugrund = "*Gier " + Ugrund
 +
  End If
 +
  If K_diff < -10 Then                                                        'Plattform rotiert zu schnell
 +
      Notaus = 1
 +
      Antriebaus = 1
 +
      Ugrund = Str(k_diff)
 +
      Ugrund = "*Gier " + Ugrund
 +
  End If
 +
'</Notauskriterien abprüfen>
 +
End If
 +
'<Akkuspannung berechnen>
 +
  Ugemessen = U_links + U_rechts
 +
  Shift Ugemessen , Right , 1 , Signed
 +
'</Akkuspannung berechnen>
 +
'<Gesamtstrom berechnen>
 +
  Igemessen = I_links + I_rechts
 +
'</Gesamtstrom berechnen>
 +
'<verbrauchte As berechnen>
 +
  If Igemessen > 0 Then
 +
      Summe_as_entladen = Summe_as_entladen + Igemessen                        'Verbrauchte
 +
  End If
 +
  If Igemessen < 0 Then
 +
      Summe_as_laden = Summe_as_laden - Igemessen                              'Rückgespeiste
 +
  End If
 +
  'mAh aus den As rausholen
 +
  'um überläufe zu verhindern
 +
  'Die 360000 kommen weil:
 +
  'Igemessen in mA -> Faktor 1
 +
  '100 Messungen pro Sekunde -> mal 100
 +
  '3600 mAmperesekunden=1 mAh -> also nochmal mal 3600
 +
  If Summe_as_entladen > 360000 Then
 +
      Summe_mah_entladen = Summe_mah_entladen + 1
 +
      Summe_as_entladen = Summe_as_entladen - 360000
 +
  End If
 +
  If Summe_as_laden > 360000 Then                                              'Das kann passieren wenns den Berg runter geht
 +
      Summe_mah_laden = Summe_mah_laden + 1
 +
      Summe_as_laden = Summe_as_laden - 360000
 +
  End If
 +
      Erzeugt = Summe_mah_laden
 +
      Verbraucht = Summe_mah_entladen
 +
      Erzeugt = Erzeugt * 0.7                                                  'Das ist der Akku-Wirkungsgrad 0.7 (0.85) bezogen auf den Stom
 +
      Verbraucht = Verbraucht - Erzeugt
 +
      Akkuinhalt = Akkukap
 +
      Akkuverbrauch = Verbraucht
 +
      Akkuprozent = Akkuverbrauch / Akkuinhalt
 +
      Akkuprozent = Akkuprozent * 1000
 +
      Akkuprozent = Int(akkuprozent)
 +
      Akkustand = Akkuprozent
 +
'</verbrauchte As berechnen>
 +
'<Strecke berechnen>
 +
  Kx = K_links + K_rechts
 +
  Kx = Kx / 2
 +
  Strecke = Kx * Di
 +
  Zentimeter = Zentimeter + Strecke
 +
  If Zentimeter >= 100 Then
 +
      Meter = Meter + 1
 +
      Zentimeter = Zentimeter - 100
 +
  End If
 +
  If Zentimeter < 0 Then
 +
      Meter = Meter - 1
 +
      Zentimeter = Zentimeter + 100
 +
  End If
 +
  If Meter >= 1000 Then
 +
      Kilometer = Kilometer + 1
 +
      Meter = Meter - 1000
 +
  End If
 +
  If Meter < 0 Then
 +
      Kilometer = Kilometer - 1
 +
      Meter = Meter + 1000
 +
  End If
 +
'</Strecke berechnen>
 +
'<Geschwindigkeit berechnen>
 +
Kv = Kv + Kx
 +
Kmh = Kv * Kmhfaktor
 +
Kv = Kv * 0.75                                                                  'Bei Änderung auch den kmhfaktor neu berechnen
 +
If Kmh > Max_v Then Max_v = Kmh
 +
If Kmh < Min_v Then Min_v = Kmh
 +
'</Geschwindigkeit berechnen>
 +
If Bzeit < 500 Then Incr Bzeit
 +
Incr Intcount
 +
Incr Tachocount
 +
Select Case Tachocount
 +
'*******************************************************************************
 +
'Alle möglichen Werte werden an den Tacho geschickt. Manche für die Funktion
 +
'des Tachos, andere um die Steuerung zu optimieren.
 +
'*******************************************************************************
 +
Case 0                                                                          'U ausgeben
 +
    B(1) = 1
 +
    U = Ugemessen
 +
    'U = Sig_Gyro_Nick
 +
    B(2) = Motormeldung(1)
 +
    B(3) = Motormeldung(2)
 +
    B(5) = 1
 +
Case 1                                                                          'I ausgeben
 +
    B(1) = 2
 +
    I = Igemessen / 10
 +
    B(2) = Motormeldung(3)
 +
    B(3) = Motormeldung(4)
 +
    B(5) = 1
 +
Case 2                                                                          'Km/h
 +
    B(1) = 3
 +
    B(2) = Kmhmeldung(1)
 +
    B(3) = Kmhmeldung(2)
 +
    B(5) = 1
 +
Case 3                                                                          'Akkuprozente
 +
    B(1) = 5
 +
    B(2) = Akkumeldung(1)
 +
    B(3) = Akkumeldung(2)
 +
    B(5) = 1
 +
Case 4                                                                          'Meter
 +
    B(1) = 7
 +
    B(2) = Metermeldung(1)
 +
    B(3) = Metermeldung(2)
 +
    B(5) = 1
 +
Case 5                                                                          'Kilometer
 +
    B(1) = 6
 +
    B(2) = Kilometermeldung(1)
 +
    B(3) = Kilometermeldung(2)
 +
    B(5) = 1
 +
Case 6
 +
      If Antriebaus = 0 Then
 +
        Maccroll = Mit_acc_roll
 +
        B(1) = 8
 +
        B(2) = Maccroll1
 +
        B(3) = Maccroll2
 +
        B(4) = 1
 +
      Else
 +
        Nullroll = Null_roll
 +
        B(1) = 12
 +
        B(2) = Nullroll1
 +
        B(3) = Nullroll2
 +
        B(4) = 1
 +
      End If
 +
Case 7
 +
      If Antriebaus = 0 Then
 +
        B(1) = 9
 +
        B(2) = Maccroll3
 +
        B(3) = Maccroll4
 +
        B(4) = 1
 +
      Else
 +
        B(1) = 13
 +
        B(2) = Nullroll3
 +
        B(3) = Nullroll4
 +
        B(4) = 1
 +
      End If
 +
Case 8
 +
      B(1) = 4
 +
      B(2) = Antriebaus
 +
      B(3) = Ta
 +
      B(4) = 1
 +
Case 9
 +
      If Antriebaus = 0 Then
 +
        maccnick = Mit_acc_nick
 +
        B(1) = 10
 +
        B(2) = maccnick1
 +
        B(3) = maccnick2
 +
        B(4) = 1
 +
      Else
 +
        nullnick = null_nick
 +
        B(1) = 14
 +
        B(2) = nullnick1
 +
        B(3) = nullnick2
 +
        B(4) = 1
 +
      End If
 +
Case 10
 +
      If Antriebaus = 0 Then
 +
        B(1) = 11
 +
        B(2) = maccnick3
 +
        B(3) = maccnick4
 +
        B(4) = 1
 +
      Else
 +
        B(1) = 15
 +
        B(2) = nullnick3
 +
        B(3) = nullnick4
 +
        B(4) = 1
 +
      End If
 +
Case 11
 +
      Sw = Stellwert
 +
      B(1) = 16
 +
      B(2) = Sw1
 +
      B(3) = Sw2
 +
      B(4) = 1
 +
Case 12
 +
      B(1) = 17
 +
      B(2) = Sw3
 +
      B(3) = Sw4
 +
      B(4) = 1
 +
Case 13
 +
      Gyronick = Sig_gyro_nick
 +
      B(1) = 18
 +
      B(2) = Gyronick1
 +
      B(3) = Gyronick2
 +
      B(4) = 1
 +
Case 14
 +
      B(1) = 19
 +
      B(2) = Gyronick3
 +
      B(3) = Gyronick4
 +
      B(4) = 1
 +
      'Tachocount = 7  'Wenn die wichtigsten Parameter schnell übermittelt werden sollen.
 +
Case 15
 +
      Kksum = K_sum
 +
      B(1) = 20
 +
      B(2) = Kksum1
 +
      B(3) = Kksum2
 +
      B(4) = 1
 +
Case 16
 +
      B(1) = 21
 +
      B(2) = Kksum3
 +
      B(3) = Kksum4
 +
      B(4) = 1
 +
If Antriebaus = 0 Then Tachocount = 255                                        'Gleich zu 0 überlaufen lassen
 +
Case 17
 +
      B(1) = 22
 +
      B(2) = Pf1
 +
      B(3) = Pf2
 +
      B(4) = 1
 +
Case 18
 +
      B(1) = 23
 +
      B(2) = Pf3
 +
      B(3) = Pf4
 +
      B(4) = 1
 +
Case 19
 +
      B(1) = 24
 +
      B(2) = If1
 +
      B(3) = If2
 +
      B(4) = 1
 +
Case 20
 +
      B(1) = 25
 +
      B(2) = If3
 +
      B(3) = If4
 +
      B(4) = 1
 +
Case 21
 +
      B(1) = 26
 +
      B(2) = Df1
 +
      B(3) = Df2
 +
      B(4) = 1
 +
Case 22
 +
      B(1) = 27
 +
      B(2) = Df3
 +
      B(3) = Df4
 +
      B(4) = 1
 +
Case 23
 +
      B(1) = 28
 +
      B(2) = Mpf1
 +
      B(3) = Mpf2
 +
      B(4) = 1
 +
Case 24
 +
      B(1) = 29
 +
      B(2) = Mpf3
 +
      B(3) = Mpf4
 +
      B(4) = 1
 +
Case 25
 +
      B(1) = 30
 +
      B(2) = Mvf1
 +
      B(3) = Mvf2
 +
      B(4) = 1
 +
Case 26
 +
      B(1) = 31
 +
      B(2) = Mvf3
 +
      B(3) = Mvf4
 +
      B(4) = 1
 +
Case 27
 +
      B(1) = 32
 +
      B(2) = Mplf1
 +
      B(3) = Mplf2
 +
      B(4) = 1
 +
Case 28
 +
      B(1) = 33
 +
      B(2) = Mplf3
 +
      B(3) = Mplf4
 +
      B(4) = 1
 +
Case 29
 +
      B(1) = 34
 +
      B(2) = Mvlf1
 +
      B(3) = Mvlf2
 +
      B(4) = 1
 +
Case 30
 +
      B(1) = 35
 +
      B(2) = Mvlf3
 +
      B(3) = Mvlf4
 +
      B(4) = 1
 +
Tachocount = 255
 +
Case Else
 +
B(5) = 0                                                                        'Nix ausgeben
 +
End Select
 +
If Intcount = 99 Then Intcount = 255
 +
If B(5) = 1 Then
 +
  'Checksumme berechnen
 +
  B(4) = B(1)
 +
  B(4) = B(4) + B(2)
 +
  B(4) = B(4) + B(3)
 +
'Print Chr(b(1)) ; Chr(b(2)) ; Chr(b(3)) ; Chr(b(4)) ; Chr(13);
 +
'Print in Assembler:
 +
$asm
 +
  LDS    R24,285                                                              'b(1)
 +
  STS    udr,R24
 +
  Warten11:
 +
  LDS    R24,UCSR0A
 +
  BST    R24,5
 +
  Brtc warten11
 +
  LDS    R24,286                                                              'b(2)
 +
  STS    udr,R24
 +
  Warten12:
 +
  LDS    R24,UCSR0A
 +
  BST    R24,5
 +
  Brtc warten12
 +
  lds    R24,287                                                              'B(3)
 +
  STS    udr,R24
 +
  Warten13:
 +
  LDS    R24,UCSR0A
 +
  BST    R24,5
 +
  Brtc warten13
 +
  LDS    R24,288                                                              'B(4)
 +
  STS    udr,R24
 +
  Warten14:
 +
  LDS    R24,UCSR0A
 +
  BST    R24,5
 +
  Brtc warten14
 +
  LDI    R24,13
 +
  STS    udr,R24
 +
  Warten15:
 +
  LDS    R24,UCSR0A
 +
  BST    R24,5
 +
  Brtc warten15
 +
$end Asm
 +
  Ta = Timer0                                                                  'Um festzustellen ob der Interrupt zu lange dauert
 +
End If
 +
Led2 = 0
 +
Return
 +
</pre>
  
 
==Tacho==
 
==Tacho==

Version vom 6. August 2011, 20:18 Uhr

Rutscherle 2 - Ein selbstbanancierender Elektroroller

Rutscherle 2

Beim Rutscherle 2 handelt es sich um einen selbstbalancierenden Elektroroller mit großer Ähnlichkeit zum bekannten Segway. Es war und ist beabsichtigt, besser zu sein als das Original. Auch wenn das bisher noch nicht ganz gelungen ist. Dies ist kein kommerzielles Projekt und soll es auch nicht werden. Noch eine Warnung: Das ist kein ungefährliches Projekt. Schon kleine Programmierfehler können sehr schmerzhafte folgen haben.

Und so fährts: Rutscherle2 auf Youtube

Technische Daten

Antrieb

2 BLDC Felgenmotoren mit je 1000W/36V. Vom Hersteller in China direkt importiert. Mittlerweile gibt es auch in Europa vergleichbare Motoren zu kaufen. Die stammen zwar auch aus China, müssen jedoch nicht selbst importiert werden. Die Motoren sind im Lieferzustand nicht direkt verwendbar. Die Sensorik muss um 3 weitere Hallsensoren erweitert werden. Die bisherige Höchstgeschwindigkeit beträgt 27km/h.

Akkus

4 Bleiakkus mit 12V/18AH von denen 3 in Reihe geschaltet sind für die Motoren, einer allein für die Steuerung und eine zukünftige Beleuchtung. Geladen werden die Akkus an einem handelsüblichen Kfz-Ladegerät. Die Reichweite beträgt auf jeden Fall mehr als 20km. Die Entscheidung für Bleiakkus wurde bewusst getroffen. Eine kWh verteuert sich um ca. 50 cent wenn sie durch den Bleiakku geht bei Litium-Polymer-Akkus sind das ca. 5 Euro. Auch sind Bleiakkus beim Laden wesentlich unempfindlicher.

Elektronik

2 Motorregler mit Atmega 168/10Mhz sowie eine Hauptsteuerung mit einem Atmega 644/20Mhz. Zusätzlich noch ein Display von Display3000 als Tacho und Datenlogger. Beschleunigungssensor: ADXL335, Gyros: LISY300AL (ArduIMU Sensor Board - Six Degrees of Freedom) Die ganze Elektronik wurde sehr einfach gehalten. (Zu mehr hats nicht gereicht) Das hat aber den Vorteil, daß keine besonderen Spezialkentnisse erforderlich sind. Wer schonmal eine Platine mit dem Bügeleisen hergestellt hat bekommt auch das hin.

Software

Die Software ist in Bascom geschrieben. Zeitkritische Teile in Assembler.

Mechanik

Der Mechanische Aufbau besteht nahezu komplett aus Aluminium. Dabei wurde darauf geachtet, dass alles mit einfachen Werkzeugen herstellbar ist. (Eine Ständerbohrmaschine sollte man schon haben.) Die Konstruktion ist so ausgelegt, dass so wenige Teile wie möglich hinter den Rädern hervorstehen. Dies verhindert Kratzer bei den ersten Fahrversuchen.

Sicherheitshinweis

Achtung! Bauen Sie sowas nur wenn Sie genau wissen was Sie machen. Es können an der Elektonik sehr hohe Ströme auftreten. Auch an den Motoren können, vor allem im Fehlerfall, sehr hohe Drehmomente entstehen. Beschweren Sie sich also nicht wenn Sie im Krankenhaus aufwachen. Sie haben es so gewollt.

Funktion

Die Funktion ist im groben recht banal. Im Prinzip macht das Teil das gleiche wie jeder Mensch beim gehen. Kommt ein Mensch nach vorn aus dem Gleichgewicht macht er einen Schritt nach vorn. Entsprechend nach hinten. Hier wird das gleiche elektronisch nachbildet. Bewegt sich die Platform nach vorn wird nach vorn beschleunigt. Die genaue technische Umsetzung wird dann doch etwas aufwändiger. Die Regelung selbst erledigt ein PID-Regler. Danach kommen jedoch noch einige zusätzliche Faktoren die z.B. die Motorkennlinie ausgleichen sowie ungewollte Richtungsänderungen bei unebenen Oberflächen reduzieren. Ähnlich aufwendig wird es bei den Motorreglern. Eine reine Blockkommutierung wie bei solchen Motoren üblich ist nur auf glatten Untergründen brauchbar. Darum müssen die Motoren um zusätzliche Hallsensoren erweitert und mit Drehstrom oder genauer Drehspannung angesteuert werden.

Elektronik

Motorregler

Der Motorregler regelt natürlich den Motor. Zusätzlich misst und übermittelt der Motorregler auch Spannung, Strom und Fahrstrecke. Die Temperatur des Kühlkörpers wird ebenfalls gemessen und wenn nötig ein Lüfter zugeschaltet. Die Ansteuerung des Motors ist keine Blockkomutierung. Diese hat sich als untauglich gezeigt. Tatsächlich wird auf den 3 Leitungen eine vereinfachte Sinusspannung ausgegeben. Das reduziert die Drehmomentwelligkeit und das Rückspeisen der Energie beim Bremsen ist deutlich besser. Ob und wieweit sich dadurch der Motorwirkungsgrad ändert wurde bisher nicht gemessen. Die ganze Einheit wird vom PC aus über die Serielle Schnittstelle (5V) parametriert und kalibriert.

Der Motorregler ist auf zwei Platinen aufgebaut. Der Grund dafür ist die einfachere Herstellung der Platinen mit der Bügeleisenmethode sowie die räumliche Trennung der hohen Ströme vom Kontroller. Ob dies wegen der Störsicherheit wirklich notwendig ist wurde aus Kostengründen nie getestet. An der Stelle ist aber eine unnötige Sicherheit besser als eine vergessene. Die Verbindung zwischen den beiden Platinen bildet ein 16-Adriges Flachkabel. Programmierstecker und der Serielle Bus werden seitlich am Kontrollerboard gesteckt. Die Verbindung zu den Hallsensoren wird über ein 6-Adriges Flachkabel hergestellt. Zusätzlich zu den beiden Platinen gehört zum Motorregler auch die im Motor verbaute Sensorplatine. Auf dieser werden die Signale der drei zusätzlichen Hallsensoren zu einem Signal zusammengefasst. Nach einem Ausfall wurde auch noch eine Zusatzplatine mit den Snubbern und den Supressordioden nachgerüstet. Ohne diese ist eine Halbbrücke nach ca. 125km Fahrt ausgefallen.

Motorregler Software

'Version 19


$prog &HFF , &HFF , &HD4 , &HF9                             'Fusebits
$regfile = "m168def.dat"
$crystal = 10000000
$baud = 57600                                               
$hwstack = 32
$swstack = 10
$framesize = 40

Declare Sub Setspeed
Declare Sub Pwm_start
Declare Sub Pwm_stop

Config Portc.0 = Input                                      'UBAT
Config Portc.1 = Input                                      'USHUNT
Config Portc.2 = Input                                      'TEMP
Config Portc.3 = Output                                     'LED1
Config Portc.4 = Output                                     'Lüfter
Config Portc.5 = Input                                      'Jumper1

Config Portd.2 = Input                                      'Hall_C
Config Portd.3 = Output                                     'A+
Config Portd.4 = Input                                      'Hall_B
Config Portd.5 = Output                                     'C+
Config Portd.6 = Output                                     'C-
Config Portd.7 = Input                                      'Hall_A
Config Portb.0 = Input                                      'Hall_D
Config Portb.1 = Output                                     'B-
Config Portb.2 = Output                                     'B+
Config Portb.3 = Output                                     'A-
Config Portb.4 = Output                                     'LED2
Config Portb.5 = Input                                      'Unbelegt (SCK)

Hall_a Alias Pind.7                                         'gelb
Hall_b Alias Pind.4                                         'blau
Hall_c Alias Pind.2                                         'Grün
Hall_d Alias Pinb.0
Led1 Alias Portc.3
Led2 Alias Portb.4
Fan Alias Portc.4
Jumper1 Alias Pinc.5

Dim Uzu As Integer At $100
Dim Izu As Integer At $102
Dim Ausgabeticks As Integer At $104                         'Anzahl Kommutieren zwischen Seriellen Telegrammen
Dim Temperatur As Integer At $106                           '
Dim Drehzahl As Integer At $108                             'Drehzahl
Dim M1 As Byte At $100 Overlay
Dim M2 As Byte At $101 Overlay
Dim M3 As Byte At $102 Overlay
Dim M4 As Byte At $103 Overlay
Dim M5 As Byte At $104 Overlay
Dim M6 As Byte At $105 Overlay
Dim M7 As Byte At $106 Overlay
Dim M8 As Byte At $107 Overlay
Dim M9 As Byte At $108 Overlay
Dim M10 As Byte At $109 Overlay
Dim Kalibrierwert As Integer At $10a
Dim Kal(2) As Byte At $10a Overlay
Dim A(15) As Byte At $10c
Dim S(12) As Byte At $11b
Dim Sinus(12) As Byte At $127
Dim Richtung(255) As Byte At $133
Dim Motorposition As Byte At $232
Dim Istrichtung As Integer At $233
Dim Altrichtung As Integer At $235
Dim Aposition As Byte At $237
Dim Zposition As Byte At $238
Dim Zwangskommutieren As Byte At $239
Dim Temp As Word At $23a
Dim Fancount As Word At $23c
Dim Motoradresse As Byte At $23e
Dim Vwinkel As Byte At $23f
Dim Rwinkel As Byte At $240
Dim Ticks As Integer At $241
Dim N As Byte At $243
Dim Notaus As Byte At $244
Dim Loopcount As Word At $245
Dim Sollrichtung As Byte At $247
Dim Altsollrichtung As Byte At $248
Dim Sollspeed As Byte At $249
Dim Altsollspeed As Byte At $24a
Dim Bdummy As Byte At $24b
Dim Intbdummy As Byte At $24c
Dim Intidummy As Integer At $24d
Dim Sdummy As Single At $24f
Dim Idummy1 As Integer At $253
Dim Idummy2 As Integer At $255
Dim Uw As Word At $257
Dim Iw As Word At $259
Dim Uwsum As Long At $25b
Dim Iwsum As Long At $25f
Dim Wcount As Long At $263
Dim Uoffset As Single At $267
Dim Ufaktor As Single At $26b
Dim Ioffset As Single At $26f
Dim Ifaktor As Single At $273
Dim Ep0 As Byte At $277
Dim Ep1 As Byte At $278
Dim Ep2 As Byte At $279
Dim Ep3 As Byte At $27a
Dim Ep4 As Byte At $27b
Dim Ep5 As Byte At $27c
Dim Ta As Byte At $27d
Dim Tb As Byte At $27e
Dim Tc As Byte At $27f
Dim Fehler As Byte At $280
Sollspeed = 0
Pwm_stop                                                    'Erstmal alles auschalten
'Variablen für die Richtungserkennung vorbelegen
'Diese Variante frisst zwar viel Arbeitsspeicher, ist aber sehr schnell
For N = 1 To 15
A(n) = 0
Next

For N = 1 To 255
Richtung(n) = 2
Next

If Jumper1 = 0 Then                                         'R
   Motoradresse = 82
   A(12) = 12
   A(4) = 11
   A(6) = 10
   A(14) = 9
   A(10) = 8
   A(2) = 7
   A(3) = 6
   A(11) = 5
   A(9) = 4
   A(1) = 3
   A(5) = 2
   A(13) = 1
   Richtung(21) = 3
   Richtung(93) = 3
   Richtung(220) = 3
   Richtung(196) = 3
   Richtung(70) = 3
   Richtung(110) = 3
   Richtung(234) = 3
   Richtung(162) = 3
   Richtung(35) = 3
   Richtung(59) = 3
   Richtung(185) = 3
   Richtung(145) = 3
   Richtung(81) = 1
   Richtung(25) = 1
   Richtung(155) = 1
   Richtung(179) = 1
   Richtung(50) = 1
   Richtung(42) = 1
   Richtung(174) = 1
   Richtung(230) = 1
   Richtung(100) = 1
   Richtung(76) = 1
   Richtung(205) = 1
   Richtung(213) = 1
Else
   Motoradresse = 76                                        'L
   A(11) = 1
   A(3) = 2
   A(1) = 3
   A(9) = 4
   A(13) = 5
   A(5) = 6
   A(4) = 7
   A(12) = 8
   A(14) = 9
   A(6) = 10
   A(2) = 11
   A(10) = 12
   Richtung(236) = 3
   Richtung(196) = 3
   Richtung(69) = 3
   Richtung(93) = 3
   Richtung(217) = 3
   Richtung(145) = 3
   Richtung(19) = 3
   Richtung(59) = 3
   Richtung(186) = 3
   Richtung(162) = 3
   Richtung(38) = 3
   Richtung(110) = 3
   Richtung(42) = 1
   Richtung(171) = 1
   Richtung(179) = 1
   Richtung(49) = 1
   Richtung(25) = 1
   Richtung(157) = 1
   Richtung(213) = 1
   Richtung(84) = 1
   Richtung(76) = 1
   Richtung(206) = 1
   Richtung(230) = 1
   Richtung(98) = 1
End If




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

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

On Timer2 Update_pwm Nosave

'Pin-Change Interrupt einstellen:
Pcmsk2 = &B10010100
Pcmsk1 = &B00000000
Pcmsk0 = &B00000001
Pcicr = &B00000101

On Pcint2 Kommutieren Nosave
On Pcint0 Kommutieren Nosave

Readeeprom Uoffset , 1
Readeeprom Ufaktor , 5
Readeeprom Ioffset , 9
Readeeprom Ifaktor , 13
'Angepasste Sinuskurve aus dem EEprom laden
For N = 1 To 12
Bdummy = N + 16
Readeeprom Sinus(n) , Bdummy
Next
'Voreilwinkel:
Readeeprom Vwinkel , 29
Readeeprom Rwinkel , 30

If Ufaktor = 0 Then Ufaktor = 1
If Ifaktor = 0 Then Ifaktor = 1


On Urxc Datenempfang Nosave
Enable Urxc
Enable Interrupts



Fancount = 15000                                            'Lüfter erstmal einschalten

Do
Led1 = 0

'<Messen>
   Uw = Getadc(0)
   Uwsum = Uwsum + Uw
   Iw = Getadc(1)
   Iwsum = Iwsum + Iw
   Incr Wcount
   Temp = Getadc(2)
'</Messen>


'<Serielle Telegramme abarbeiten>
If Ep0 = 13 Then
   If Ep5 = 13 Then
      If Ep1 = Motoradresse Then                            '"R", "L"
         Disable Urxc
         $asm
         lds R24,{EP1}
         LDs R25,{EP2}
         add R24,R25
         LDs R25,{EP3}
         add R24,R25
         STS {Bdummy},R24
         $end Asm
            If Bdummy = Ep4 Then                            'Die Prüfsumme stimmt
            Led1 = 1
            Pcicr = &B00000000                              'Kommutieren kurz deaktivieren, weil sonst die Ticks während dem Zugriff geändert werden könnten
            Ausgabeticks = Ticks
            Ticks = 0
            Pcicr = &B00000101

            $asm
               lds R24,{M1}
               LDs R25,{M2}
               add R24,R25
               LDs R25,{M3}
               add R24,R25
               LDs R25,{M4}
               add R24,R25
               LDs R25,{M5}
               add R24,R25
               LDs R25,{M6}
               add R24,R25
               STS {Bdummy},R24
               'Antwort ausgeben
               'Print Chr(m1) ; Chr(m2) ; Chr(m3) ; Chr(m4) ; Chr(m5) ; Chr(m6) ; Chr(bdummy) ; Chr(13) ;
               LdS     R24,{M1}
               STS     udr,R24
               Warten1:
               LDS     R24,UCSR0A
               BST     R24,5
               Brtc warten1
               LDS     R24,{M2}
               STS     udr,R24
               Warten2:
               LDS     R24,UCSR0A
               BST     R24,5
               Brtc warten2
               lds     R24,{M3}
               STS     udr,R24
               Warten3:
               LDS     R24,UCSR0A
               BST     R24,5
               Brtc warten3
               LDS     R24,{M4}
               STS     udr,R24
               Warten4:
               LDS     R24,UCSR0A
               BST     R24,5
               Brtc warten4
               LDS     R24,{M5}
               STS     udr,R24
               Warten5:
               LDS     R24,UCSR0A
               BST     R24,5
               Brtc warten5
               LDS     R24,{M6}
               STS     udr,R24
               Warten6:
               LDS     R24,UCSR0A
               BST     R24,5
               Brtc warten6
               LDS     R24,{bdummy}
               STS     udr,R24
               Warten7:
               LDS     R24,UCSR0A
               BST     R24,5
               Brtc warten7
               LDi     R24,13
               STS     udr,R24
               Warten8:
               LDS     R24,UCSR0A
               BST     R24,5
               Brtc warten8
            $end Asm
            If Ep2 < 3 Then
               Notaus = 0
               Loopcount = 0
               Sollrichtung = Ep2
               Sollspeed = Ep3
               If Sollrichtung = 2 Then
                  Sollrichtung = 1
                  Notaus = 1
                  Loopcount = 250
                  Sollspeed = 0
               End If
               Setspeed
               '<Messwerte berechnen>
               'Messwerte werden hier berechnet, weil genau jetzt 1/100s Zeit ist.
               'Spannung berechnen
               Sdummy = Uwsum / Wcount
               Sdummy = Sdummy - Uoffset
               Sdummy = Sdummy * Ufaktor
               Uzu = Int(sdummy)
               Uwsum = 0
               'Strom berechnen
               Sdummy = Iwsum / Wcount
               Sdummy = Sdummy - Ioffset
               Sdummy = Sdummy * Ifaktor
               Izu = Int(sdummy)
               Iwsum = 0
               Wcount = 0
               '</Messwerte berechnen>
            Else
               Select Case Ep2
                  Case 3                                    'Sinus(1) empfangen
                     Sinus(1) = Ep3
                     Writeeeprom Sinus(1) , 17
                  Case 4                                    'Sinus(2) empfangen
                     Sinus(2) = Ep3
                     Writeeeprom Sinus(2) , 18
                  Case 5                                    'Sinus(3) empfangen
                     Sinus(3) = Ep3
                     Writeeeprom Sinus(3) , 19
                  Case 6                                    'Sinus(4) empfangen
                     Sinus(4) = Ep3
                     Writeeeprom Sinus(4) , 20
                  Case 7                                    'Sinus(5) empfangen
                     Sinus(5) = Ep3
                     Writeeeprom Sinus(5) , 21
                  Case 8                                    'Sinus(6) empfangen
                     Sinus(6) = Ep3
                     Writeeeprom Sinus(6) , 22
                  Case 9                                    'Sinus(7) empfangen
                     Sinus(7) = Ep3
                     Writeeeprom Sinus(7) , 23
                  Case 10                                   'Sinus(8) empfangen
                     Sinus(8) = Ep3
                     Writeeeprom Sinus(8) , 24
                  Case 11                                   'Sinus(9) empfangen
                     Sinus(9) = Ep3
                     Writeeeprom Sinus(9) , 25
                  Case 12                                   'Sinus(10) empfangen
                     Sinus(10) = Ep3
                     Writeeeprom Sinus(10) , 26
                  Case 13                                   'Sinus(11) empfangen
                     Sinus(11) = Ep3
                     Writeeeprom Sinus(11) , 27
                  Case 14                                   'Sinus(12) empfangen
                     Sinus(12) = Ep3
                     Writeeeprom Sinus(12) , 28
                  Case 15                                   'Spannung Kalibrieren (Byte 1)
                     Kal(1) = Ep3
                  Case 16                                   'Spannung Kalibrieren (Byte 2)
                     Kal(2) = Ep3
                     Sdummy = Uwsum / Wcount
                     Ufaktor = Kalibrierwert / Sdummy
                     Writeeeprom Ufaktor , 5
                     Uoffset = 0                            'Schaltungsbedingt
                     Writeeeprom Uoffset , 1
                  Case 17                                   'Strom Kalibrieren (Byte 1)
                     Kal(1) = Ep3
                  Case 18                                   'Strom Kalibrieren Byte (2)
                     Kal(2) = Ep3
                     Sdummy = Iwsum / Wcount
                     If Kalibrierwert = 0 Then              'Das ist der Offset
                        Ioffset = Sdummy + 1
                        Writeeeprom Ioffset , 9
                     Else                                   'Jetzt den Faktor berechnen
                        Sdummy = Sdummy - Ioffset
                        Ifaktor = Kalibrierwert / Sdummy
                        Writeeeprom Ifaktor , 13
                     End If
                  Case 19
                     Vwinkel = Ep3
                     Writeeeprom Vwinkel , 29
                  Case 20
                     Rwinkel = Ep3
                     Writeeeprom Rwinkel , 30

               End Select
            End If

            $asm                                            'Empfangspuffer löschen
               LDI     R24,0
               sTS     {EP1},R24
               sTS     {EP2},R24
               sTS     {EP3},R24
               sTS     {EP4},R24
            $end Asm

            End If
         Enable Urxc

      End If
   End If
End If
'</Serielle Telegramme abarbeiten>



'<Lüfter schalten>
   If Temp > 740 Then Fancount = 15000                      'Schaltet den Lüfter ein
   If Fancount > 0 Then
      Fan = 1
   Else
      Fan = 0
   End If
   If Fancount > 0 Then Decr Fancount
'</Lüfter schalten>


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


'********************************************************************************************
Kommutieren:
$asm
      PUSH    R10
      PUSH    R11
      PUSH    R16
      PUSH    R17
      PUSH    R20
      PUSH    R21
      PUSH    R24
      PUSH    R26
      PUSH    R27
      IN      R24,sreg
      PUSH    R24
      clr R16
      lds R17, $0023
      BST R17,0
      BLD R16,3
      LDS R17, $0029
      BST R17,2
      bld R16,2
      BST R17,4
      bld R16,1
      BST R17,7
      bld R16,0
      lds R17,{aposition}
      !swap R17
      andi R17,&B11110000
      add R17,R16
      STS {Aposition},R17
      CLR     R17
      LDI     R26,$0B
      LDI     R27,$01
      ADD     R26,R16
      ADC     R27,R17
      LD      R24,X
      LDS     R16,{Sollrichtung}
      CPI     R16,0
      BREQ    Sollrichtungnull
      Jmp     Sollrichtungnichtnull
      Sollrichtungnull:
      LDS     R20,{Vwinkel}
      Add     R24,R20
      JMP     Richtunggesetzt
      Sollrichtungnichtnull:
      LDS     R20,{Rwinkel}
      Add     R24,R20
      Richtunggesetzt:
      CPI     R24,$0D
      BRCC    Istgroesser13_1
      JMP     Istkleiner13_1
      Istgroesser13_1:
      SUBI    R24,$0C
      Istkleiner13_1:
      CLR     R20
      Ldi     R26,$1a
      LDI     R27,$01
      ADD     R26,R24
      ADC     R27,R20
      LD      R16,X
      STS     {Ta},R16
      subi     R24,$fc
      CPI     R24,$0D
      BRCC    Istgroesser13_2
      JMP     Istkleiner13_2
      Istgroesser13_2:
      SUBI    R24,$0C
      Istkleiner13_2:
      CLR     R20
      Ldi     R26,$1a
      LDI     R27,$01
      ADD     R26,R24
      ADC     R27,R20
      LD      R16,X
      STS     {Tc},R16
      subi     R24,$fc
      CPI     R24,$0D
      BRCC    Istgroesser13_3
      JMP     Istkleiner13_3
      Istgroesser13_3:
      SUBI    R24,$0C
      Istkleiner13_3:
      CLR     R20
      Ldi     R26,$1a
      LDI     R27,$01
      ADD     R26,R24
      ADC     R27,R20
      LD      R16,X
      STS     {Tb},R16
      lds     R10,{Aposition}
      CLR     R11
      LDI     R26,$32
      LDI     R27,$01
      ADD     R26,R10
      ADC     R27,R11
      LD      R16,X+
      CLR     R17
      LDI     R20,$02
      LDI     R21,$00
      !Sub R16 , R20
      SBC     R17,R21
      LDI     R26,$33
      LDI     R27,$02
      ST      X+,R16
      ST      X,R17
      LDI     R26,$41
      LDI     R27,$02
      LD      R16,X+
      LD      R17,X
      LDI     R26,$33
      LDI     R27,$02
      LD      R20,X+
      LD      R21,X
      ADD     R16,R20
      ADC     R17,R21
      LDI     R26,$41
      LDI     R27,$02
      ST      X+,R16
      ST      X,R17
      LDS     R24,timsk2
      ORI     R24,$01
      STS     timsk2,R24
      POP     R24
      !Out sreg , R24
      POP     R27
      POP     R26
      POP     R24
      POP     R21
      POP     R20
      POP     R17
      POP     R16
      POP     R11
      POP     R10
$end Asm

Return
'********************************************************************************************





'********************************************************************************************
Update_pwm:
$asm
      PUSH    R24
      IN      R24,sreg
      PUSH    R24
      lds R24,{Ta}
      STS     ocr2a,R24
      STS     ocr2b,R24
      LDS R24,{Tb}
      STS     ocr1al,R24
      STS     ocr1bl,R24
      lds R24,{Tc}
      !OUT    ocr0a,R24
      !OUT    ocr0b,R24
      LDS     R24,timsk2
      ANDI    R24,$FE
      STS     timsk2,R24
      POP     R24
      !OUT    sreg,R24
      POP     R24
$end Asm
Return
'********************************************************************************************


'********************************************************************************************
Datenempfang:
$asm
PUSH R24;
IN R24, SREG;
PUSH R24;
LDS R24,{EP1}
STS {EP0},R24
LDS R24,{EP2}
STS {EP1},R24
LDS R24,{EP3}
STS {EP2},R24
LDS R24,{EP4}
STS {EP3},R24
LDS R24,{EP5}
STS {EP4},R24
IN  R24,UDR
STs {EP5},R24
POP R24;
Out Sreg , R24;
POP R24;
$end Asm
Return
'********************************************************************************************

'********************************************************************************************
Sub Setspeed



If Notaus = 1 Then
   Pwm_stop
   Sollspeed = 0
   For N = 1 To 12
      S(n) = 0
   Next
Else
   For N = 1 To 12
      Idummy1 = Sollspeed * Sinus(n)
      Idummy1 = Idummy1 / 100
      S(n) = Idummy1
   Next
   If Tccr0a = 0 Then                                       'Wenn die PWM aus ist
      Pwm_start
   End If
   '<Zwangskommutieren>
   Zwangskommutieren = 0
   If Sollrichtung <> Altsollrichtung Then Zwangskommutieren = 1
   If Sollspeed <> Altsollspeed Then Zwangskommutieren = 1

   If Zwangskommutieren = 1 Then
      Pcicr = &B00000000                                    'Den Eigentlichen Kommutierinterrupt abschalten
         $asm
         clr R16
         lds R17, $0023
         BST R17,0
         BLD R16,3
         LDS R17, $0029
         BST R17,2
         bld R16,2
         BST R17,4
         bld R16,1
         BST R17,7
         bld R16,0
         lds R17,{aposition}
         !swap R17
         andi R17,&B11110000
         add R17,R16
         STS {Aposition},R17
         CLR     R17
         LDI     R26,$0B
         LDI     R27,$01
         ADD     R26,R16
         ADC     R27,R17
         LD      R24,X
         LDS     R16,{Sollrichtung}
         CPI     R16,0
         BREQ    Sollrichtungnullz
         Jmp     Sollrichtungnichtnullz
         Sollrichtungnullz:
         LDS     R20,{Vwinkel}
         Add     R24,R20
         JMP     Richtunggesetztz
         Sollrichtungnichtnullz:
         LDS     R20,{Rwinkel}
         Add     R24,R20
         Richtunggesetztz:
         CPI     R24,$0D
         BRCC    Istgroesser13_1z
         JMP     Istkleiner13_1z
         Istgroesser13_1z:
         SUBI    R24,$0C
         Istkleiner13_1z:
         CLR     R20
         Ldi     R26,$1a
         LDI     R27,$01
         ADD     R26,R24
         ADC     R27,R20
         LD      R16,X
         STS     {Ta},R16
         subi     R24,$fc
         CPI     R24,$0D
         BRCC    Istgroesser13_2z
         JMP     Istkleiner13_2z
         Istgroesser13_2z:
         SUBI    R24,$0C
         Istkleiner13_2z:
         CLR     R20
         Ldi     R26,$1a
         LDI     R27,$01
         ADD     R26,R24
         ADC     R27,R20
         LD      R16,X
         STS     {Tc},R16
         subi     R24,$fc
         CPI     R24,$0D
         BRCC    Istgroesser13_3z
         JMP     Istkleiner13_3z
         Istgroesser13_3z:
         SUBI    R24,$0C
         Istkleiner13_3z:
         CLR     R20
         Ldi     R26,$1a
         LDI     R27,$01
         ADD     R26,R24
         ADC     R27,R20
         LD      R16,X
         STS     {Tb},R16
         $end Asm
      Pcicr = &B00000101                                    'Den Eigentlichen Kommutierinterrupt einschalten
      'Nur Overflow Interrupt (Seite 105)
      Timsk2.toie2 = 1

      Toggle Led2

   End If

   '</Zwangskommutieren>
End If
Altsollrichtung = Sollrichtung
Altsollspeed = Sollspeed
End Sub
'********************************************************************************************

'********************************************************************************************
Sub Pwm_start
Disable Interrupts                                          'Damit die Timer auch wirklich synchron laufen
   Tccr0a = 161
   Tccr0b = 1
   Ocr0a = 0
   Ocr0b = 0
   Tccr1a = 161
   Tccr1b = 1
   Ocr1ah = 0
   Ocr1al = 0
   Ocr1bh = 0
   Ocr1bl = 0
   Tccr2a = 161
   Tccr2b = 1
   Ocr2a = 0
   Ocr2b = 0
   Tcnt0 = 0                                                'Timer Startwert, wegen Synchron
   Tcnt1 = 6                                                'Timer Startwert, wegen Synchron
   Tcnt2 = 9                                                'Timer Startwert, wegen Synchron
Enable Interrupts
End Sub
'********************************************************************************************


'********************************************************************************************
Sub Pwm_stop
   Tccr0a = 0
   Tccr0b = 0
   Ocr0a = 0
   Ocr0b = 0
   Tccr1a = 0
   Tccr1b = 0
   Ocr1ah = 0
   Ocr1al = 0
   Ocr1bh = 0
   Ocr1bl = 0
   Tccr2a = 0
   Tccr2b = 0
   Ocr2a = 0
   Ocr2b = 0
   'alles aus
   Portd.3 = 0                                              'A+
   Portb.3 = 1                                              'A-
   Portb.2 = 0                                              'B+
   Portb.1 = 1                                              'B-
   Portd.5 = 0                                              'C+
   Portd.6 = 1                                              'C-
End Sub
'********************************************************************************************

Hauptsteuerung

Die Hauptsteuerung erledigt die gesamte Lageregelung. Sie steuert die Motoren und gibt die entsprechenden Meldungen an den Tacho/Datenlogger aus. Sie stellt auch die 5V für den Tacho bereit. Darum die Möglichkeit einen größeren Spannungsregler zu bestücken. Die Lageregelung speichert auch die gefahrenen Km, einigen Min-Max Werte und diverse Werte bei einem Notaus. Diese Werte werden im EEprom als Text abgelegt, also direkt lesbar.

Sensoradapter

Zur Hauptsteuerung gehört auch der Sensoradapter. (Schaltbild siehe Hauptsteuerung) Dieser stellt die 3,3V für die Sensoren bereit. Im Unterschied zu ähnlichen Projekten besitzt das Rutscherle2 kein Potentiometer für die Lenkung. Zum Lenken werden die gesamten Sensoren gekippt. Darum auch dieser Sensoradapter. Die Sensoren werden mit dem Adapter direkt mit der Achse der Lenkung verbunden. Die ideale Position ist dabei natürlich genau im Schnittpunkt von Lenk- und Motorachse.

Software Version 66

$prog &HFF , &HFF , &HD1 , &HFF                                                 'Fusebits für Atmega644
$regfile = "m644def.dat"
$framesize = 64
$swstack = 64
$hwstack = 64
$crystal = 20000000
$baud = 57600
Declare Sub Onbutton1
Declare Sub Logtext(byval Adresse As Word , Text As String )
Declare Sub Akkugrenzen
Declare Sub Unfalldatenschreiber
Declare Sub Betriebsdatenschreiber
Config Portb.0 = Output
Config Portb.1 = Output
Config Portb.2 = Output
Config Portb.3 = Output
Config Portb.4 = Output
Config Portb.6 = Input                                                          'Fusstaster
Config Portc.0 = Input
Config Portc.1 = Input
Config Portc.2 = Input
Config Portc.3 = Input
Config Portc.4 = Input
Config Portc.5 = Input
Config Portc.6 = Input
Config Portc.7 = Input
Config Portd.7 = Output
Config Portd.6 = Output
Config Portd.5 = Output
Config Portd.4 = Output
Config Portd.3 = Output
Config Portd.2 = Input
Led1 Alias Portb.4
Led2 Alias Portb.3
Sensor_aus Alias Portb.2
Sensor_test Alias Portb.1                                                       'Self-Test. 0=Normalbetrieb
Led3 Alias Portd.4
Led4 Alias Portd.5
Led5 Alias Portd.6
Led6 Alias Portd.3
Led7 Alias Portd.7
Button1 Alias Pind.2
Fusstaster Alias Pinb.6
'******************************************************************************************************************************
'******************************************************************************************************************************
'Konstanten für die Geschwindigkeits und Streckenmessung
Const Kom_pro_u = 276                                                           '276*Kommutieren für eine Umdrehung.
Const Radumpfang = 139                                                          'RaduMPFang in Zentimeter
'Maximalwert bis zu dem die Abweichung 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 Vmax = 60                                                                 'Vmax Vorwärts in km/h*10
Const Vmaxr = -60                                                               'Vmax Rückwärts in km/h*10
'Konstanten für die Akkus
Const Akkuleer = 3600                                                           '36V, 12V pro Akku -> Akkus leer
Const Akkufastleer = 3750
'Konstanten für die Signalaufbereitoung
Const Gyro_anz = 10000
Const Acc_anz = 50
Const Gyro_faktor = 91
'******************************************************************************************************************************
'******************************************************************************************************************************
'Variablen für die Rückmeldungen von den Motorreglern
Dim U As Integer At $100
Dim I As Integer At $102
Dim K As Integer At $104
Dim Checksumme As Byte At $106
Dim Ende As Byte At $107
Dim Motormeldung(8) As Byte At $100 Overlay
Dim M1 As Byte At $100 Overlay
Dim M2 As Byte At $101 Overlay
Dim M3 As Byte At $102 Overlay
Dim M4 As Byte At $103 Overlay
Dim M5 As Byte At $104 Overlay
Dim M6 As Byte At $105 Overlay
Dim M7 As Byte At $106 Overlay
Dim M8 As Byte At $107 Overlay
'Variablen für die Telegramme an die Motorregler
Dim Checksum_rechts As Byte At $108
Dim Checksum_links As Byte At $109
Dim Richtung_rechts As Byte At $10a
Dim Richtung_links As Byte At $10b
Dim Pwm_rechts As Byte At $10c
Dim Pwm_links As Byte At $10d
Dim Zeiger_meldung As Byte At $10e
Dim Ta As Byte At $10f
Dim U_links As Integer At $111
Dim U_rechts As Integer At $113
Dim I_links As Integer At $115
Dim I_rechts As Integer At $117
Dim K_links As Integer At $119
Dim K_rechts As Integer At $11b
Dim B(5) As Byte At $11d
Dim Kilometer As Word At $122
Dim Kilometermeldung(2) As Byte At $122 Overlay
Dim Akkustand As Integer At $124
Dim Akkumeldung(2) As Byte At $124 Overlay
Dim Kmh As Integer At $126
Dim Kmhmeldung(2) As Byte At $126 Overlay
Dim Meter As Integer At $128
Dim Metermeldung(2) As Byte At $128 Overlay
Dim Di As Single At $12a                                                        'Distanz pro Kommutieren
Dim Kmhfaktor As Single At $12e
Dim Zentimeter As Single At $132
Dim Strecke As Single At $136                                                   'Gefahrene Strecke während 1/100s
Dim Kx As Single At $13a                                                        'Anzahl Kommutierungen für die Streckenberechnung
Dim Kv As Single At $13e                                                        'Anzahl Kommutierungen für die Geschwindigkeitosberechnung
Dim Vbrems As Long At $142                                                      'Bremse bei Überlast oder zu hoher Geschwindigkeit
Dim Lbrems As Long At $146
Dim Brems As Long At $14a
Dim P_faktor As Single At $14e
Dim Pf1 As Byte At $14e Overlay
Dim Pf2 As Byte At $14f Overlay
Dim Pf3 As Byte At $150 Overlay
Dim Pf4 As Byte At $151 Overlay
Dim I_faktor As Single At $152
Dim If1 As Byte At $152 Overlay
Dim If2 As Byte At $153 Overlay
Dim If3 As Byte At $154 Overlay
Dim If4 As Byte At $155 Overlay
Dim D_faktor As Single At $156
Dim Df1 As Byte At $156 Overlay
Dim Df2 As Byte At $157 Overlay
Dim Df3 As Byte At $158 Overlay
Dim Df4 As Byte At $159 Overlay
Dim Gyro_nick As Word At $015a
Dim Gyro_roll As Word At $015c
Dim Gyro_roll_alt As Word At $015e
Dim Gyro_nick_alt As Word At $0160
Dim acc_nick As Long At $0162
Dim Acc_roll As Long At $0166
Dim Sum_acc_roll As Long At $016a
Dim Sum_acc_nick As Long At $016e
Dim Sum_gyro_roll As Long At $0172
Dim Sum_gyro_nick As Long At $0176
Dim Korr_acc_roll As Long At $017a
Dim Korr_acc_nick As Long At $017e
Dim Maccroll As Long At $0182
Dim Maccroll1 As Byte At $0182 Overlay
Dim Maccroll2 As Byte At $0183 Overlay
Dim Maccroll3 As Byte At $0184 Overlay
Dim Maccroll4 As Byte At $0185 Overlay
Dim Maccnick As Long At $0186
Dim maccnick1 As Byte At $0186 Overlay
Dim maccnick2 As Byte At $0187 Overlay
Dim maccnick3 As Byte At $0188 Overlay
Dim maccnick4 As Byte At $0189 Overlay
Dim Mit_acc_nick_s As Long At $018a
Dim Mit_gyro_roll As Long At $018e
Dim Mit_gyro_nick As Long At $0192
Dim Sig_gyro_roll As Long At $0196
Dim Sig_gyro_nick As Long At $019a

Dim Nullroll As Long At $019e
Dim Nullroll1 As Byte At $019e Overlay
Dim Nullroll2 As Byte At $019f Overlay
Dim Nullroll3 As Byte At $01a0 Overlay
Dim Nullroll4 As Byte At $01a1 Overlay
Dim Nullnick As Long At $01a2
Dim nullnick1 As Byte At $01a2 Overlay
Dim nullnick2 As Byte At $01a3 Overlay
Dim nullnick3 As Byte At $01a4 Overlay
Dim nullnick4 As Byte At $01a5 Overlay
Dim Stellwert As Single At $01a6
Dim Mrg As Single At $01aa
Dim Mrg1 As Byte At $01aa Overlay
Dim Mrg2 As Byte At $01ab Overlay
Dim Mrg3 As Byte At $01ac Overlay
Dim Mrg4 As Byte At $01ad Overlay
Dim Mra As Single At $01ae
Dim Mra1 As Byte At $01ae Overlay
Dim Mra2 As Byte At $01af Overlay
Dim Mra3 As Byte At $01b0 Overlay
Dim Mra4 As Byte At $01b1 Overlay
Dim Kksum As Single At $01b2
Dim Kksum1 As Byte At $01b2 Overlay
Dim Kksum2 As Byte At $01b3 Overlay
Dim Kksum3 As Byte At $01b4 Overlay
Dim Kksum4 As Byte At $01b5 Overlay
Dim Sw As Single At $01b6
Dim Sw1 As Byte At $01b6 Overlay
Dim Sw2 As Byte At $01b7 Overlay
Dim Sw3 As Byte At $01b8 Overlay
Dim Sw4 As Byte At $01b9 Overlay
Dim Gyronick As Long At $01ba
Dim Gyronick1 As Byte At $01ba Overlay
Dim Gyronick2 As Byte At $01bb Overlay
Dim Gyronick3 As Byte At $01bc Overlay
Dim Gyronick4 As Byte At $01bd Overlay
Dim Mp_faktor As Single At $01be
Dim Mpf1 As Byte At $01be Overlay
Dim Mpf2 As Byte At $01bf Overlay
Dim Mpf3 As Byte At $01c0 Overlay
Dim Mpf4 As Byte At $01c1 Overlay
Dim Mv_faktor As Single At $01c2
Dim Mvf1 As Byte At $01c2 Overlay
Dim Mvf2 As Byte At $01c3 Overlay
Dim Mvf3 As Byte At $01c4 Overlay
Dim Mvf4 As Byte At $01c5 Overlay
Dim Mpl_faktor As Single At $01c6
Dim Mplf1 As Byte At $01c6 Overlay
Dim Mplf2 As Byte At $01c7 Overlay
Dim Mplf3 As Byte At $01c8 Overlay
Dim Mplf4 As Byte At $01c9 Overlay
Dim Mvl_faktor As Single At $01ca
Dim Mvlf1 As Byte At $01ca Overlay
Dim Mvlf2 As Byte At $01cb Overlay
Dim Mvlf3 As Byte At $01cc Overlay
Dim Mvlf4 As Byte At $01cd Overlay
Dim P_tacho As Single At $01ce
Dim I_tacho As Single At $01d2
Dim D_tacho As Single At $01d6
Dim Mp_tacho As Single At $01da
Dim Mv_tacho As Single At $01de
Dim Mpl_tacho As Single At $01e2
Dim Mvl_tacho As Single At $01e6
Dim Tdummy As Integer At $01ea
Dim Tachomeldung(30) As Byte At $01ce Overlay
Dim T1 As Byte At $01ce Overlay
Dim T2 As Byte At $01cf Overlay
Dim T3 As Byte At $01d0 Overlay
Dim T4 As Byte At $01d1 Overlay
Dim T5 As Byte At $01d2 Overlay
Dim T6 As Byte At $01d3 Overlay
Dim T7 As Byte At $01d4 Overlay
Dim T8 As Byte At $01d5 Overlay
Dim T9 As Byte At $01d6 Overlay
Dim T10 As Byte At $01d7 Overlay
Dim T11 As Byte At $01d8 Overlay
Dim T12 As Byte At $01d9 Overlay
Dim T13 As Byte At $01da Overlay
Dim T14 As Byte At $01db Overlay
Dim T15 As Byte At $01dc Overlay
Dim T16 As Byte At $01dd Overlay
Dim T17 As Byte At $01de Overlay
Dim T18 As Byte At $01df Overlay
Dim T19 As Byte At $01e0 Overlay
Dim T20 As Byte At $01e1 Overlay
Dim T21 As Byte At $01e2 Overlay
Dim T22 As Byte At $01e3 Overlay
Dim T23 As Byte At $01e4 Overlay
Dim T24 As Byte At $01e5 Overlay
Dim T25 As Byte At $01e6 Overlay
Dim T26 As Byte At $01e7 Overlay
Dim T27 As Byte At $01e8 Overlay
Dim T28 As Byte At $01e9 Overlay
Dim T29 As Byte At $01ea Overlay
Dim T30 As Byte At $01eb Overlay
Dim Werteneu As Byte
Dim Mit_acc_roll As Long
Dim Mit_acc_nick As Long
Dim Null_roll As Long
Dim Null_nick As Long
Dim Mit_regelzeit_ges As Single
Dim Lagefehler As Long
Dim Regelzeit As Word                                                           'Die Zeit in 1/100s die gebraucht wird bis die Nullage erreicht ist
Dim Regelzeitmax As Word
Dim Mit_acc_roll_max As Long
Dim Mit_acc_nick_max As Long
Dim Mit_acc_roll_min As Long
Dim Mit_acc_nick_min As Long
Dim Pwm_gesamt As Single
Dim Pwm_rechts_max As Byte
Dim Pwm_links_max As Byte
Dim Intcount As Byte
Dim K_diff As Single
Dim K_sum As Single
Dim K_diff_min As Single
Dim K_diff_max As Single
Dim Pwm_lenk_rechts As Integer
Dim Pwm_lenk_links As Integer
Dim Lenkvorgabe As Single
Dim Mp_anpassung As Single
Dim Mv_anpassung As Single
Dim Mpl_anpassung As Single
Dim Mvl_anpassung As Single
Dim K_sum_anpassung As Single
Dim Wdummy1 As Word
Dim Ldummyi As Long
Dim Ldummy1 As Long
Dim Ldummy2 As Long
Dim Bdummy1 As Byte
Dim Idummy1 As Integer
Dim Sdummy1 As Single
Dim Antriebaus As Byte
Dim Notaus As Byte
Dim Sensor_ok As Byte                                                           'Zeigt an ob die Mittelwerte der Gyros stabil stehen.
Dim Sensor_ok_count As Byte
Dim Ltest As Byte
Dim Ugemessen As Long                                                           'Batteriespannung in V*100
Dim Igemessen As Long                                                           'Gesamter Motorstrom in mA
Dim Imin As Long                                                                'Kleinster gemessener Motorstrom (größte Rückspeisung)
Dim Imax As Long                                                                'größter gemessener Motorstrom
Dim Summe_as_entladen As Long
Dim Summe_mah_entladen As Long
Dim Summe_as_laden As Long
Dim Summe_mah_laden As Long
Dim Akkukap As Long
Dim Akku1 As Long
Dim Akku2 As Long
Dim Akku3 As Long
Dim Akku4 As Long
Dim Akku5 As Long
Dim Erzeugt As Long
Dim Verbraucht As Long
'Single Variablen um die Prozente rechnen zu können
Dim Akkuinhalt As Single
Dim Akkuverbrauch As Single
Dim Akkuprozent As Single
Dim Adresse As Word
Dim Bzeit As Long
Dim Bstate As Byte
Dim Frei As Byte
Dim Timeout As Byte
Dim S_err As Byte
Dim Seriell_err_l As Word
Dim Seriell_err_r As Word
Dim Tamax As Byte
Dim Fusscount As Word
Dim Blinkcount As Byte
Dim Lenkerstellung As Single
Dim Lenkstellwert As Single
Dim Lenkfehler As Single
Dim N As Integer
Dim Utext As String * 32                                                        'Texte für den Unfalldatenschreiber (als Text im eeprom)
Dim Ugrund As String * 32
Dim Text As String * 16
Dim Min_v As Integer
Dim Max_v As Integer
Dim Tachocount As Byte
Led3 = 1
Led4 = 1
Led5 = 1
Led6 = 1
Led7 = 1
Wait 5                                                                          '5 Sekunden Warten
Antriebaus = 1
'Ein paar feste Werte vorab berechnen:
Di = Radumpfang / Kom_pro_u
Kmhfaktor = Di * 9                                                              'Eigentlich 3.6 aber bei der Mittelwertbildung entsteht der 4-Fache Wert. Darum 9, und es ergibt sich automateisch V in 0.1km/h
'Unbenutzte Pins auf Masse legen
Portb.7 = 0
Portb.5 = 0
'Input-Pins hochziehen
Portc.0 = 1
Portc.1 = 1
Portc.2 = 1
Portc.3 = 1
Portc.4 = 1
Portc.5 = 1
Portc.6 = 1
'Sensorboard schalten:
Sensor_aus = 0
Sensor_test = 0
'Hochziehwiderstände der Knöpfe einschalten
Button1 = 1
Fusstaster = 1
Config Adc = Single , Prescaler = Auto , Reference = Internal_2.56
Start Adc
'Wartezeit bis die Motorregler soweit sind
Readeeprom Max_v , $2f0
Readeeprom Min_v , $2f2
Readeeprom Summe_as_entladen , $300                                             'Entnommene Wh und Ws aus EEPROM lesen
Led3 = 0
Waitms 150
Readeeprom Summe_mah_entladen , $310
Led4 = 0
Waitms 150
Readeeprom Summe_as_laden , $320
Led5 = 0
Waitms 150
Readeeprom Summe_mah_laden , $330
Led6 = 0
Waitms 150
Readeeprom Akkukap , $340
Led7 = 0
Waitms 150
Readeeprom Kilometer , $350
Readeeprom Meter , $360
Readeeprom Zentimeter , $370
'<Faktoren für den PID-Regler einlesen>
      Readeeprom P_faktor , $380
      Readeeprom I_faktor , $384
      Readeeprom D_faktor , $388
      Readeeprom Mp_faktor , $38c
      Readeeprom Mv_faktor , $390
      Readeeprom Mpl_faktor , $394
      Readeeprom Mvl_faktor , $398
      If P_faktor < 0 Or P_faktor > 1 Then P_faktor = 0.0033                    'Halbwegs sinnvolle Werte vorbelegen
      If I_faktor < 0 Or I_faktor > 1 Then I_faktor = 0.00006
      If D_faktor < 0 Or D_faktor > 1 Then D_faktor = 0.08
      If Mp_faktor < 0 Or Mp_faktor > 1 Then Mp_faktor = 0.006
      If Mv_faktor < 0 Or Mv_faktor > 1 Then Mv_faktor = 0
      If Mpl_faktor < 0 Or Mpl_faktor > 1 Then Mpl_faktor = 0
      If Mvl_faktor < 0 Or Mvl_faktor > 1 Then Mvl_faktor = 0
'</Faktoren für den PID-Regler einlesen>
If Kilometer = $ffff Then                                                       'Kommt bei frischem EEPROM vor
Zentimeter = 0
Meter = 0
Kilometer = 0
Min_v = 0
Max_v = 0
End If
'Bei Fehlerhaften Werten aus dem EEPROM einfach 100mAh als Vorgabe
If Akkukap <= 0 Then Akkukap = 100
If Akkukap > 32000 Then Akkukap = 100
Akkugrenzen
Antriebaus = 1                                                                  'Beim Start ist immer alles aus
Notaus = 0
Readeeprom Max_v , $2f0
Readeeprom Min_v , $2f4
Config Watchdog = 128
'<Sicherheitstest 1>
      Start Watchdog
      If Button1 = 0 Then Wait 10
      If Antriebaus = 0 Then Wait 10
      If P_faktor > 1 Then Wait 10
      If I_faktor > 1 Then Wait 10
      If D_faktor > 1 Then Wait 10
      If P_faktor < 0 Then Wait 10
      If I_faktor < 0 Then Wait 10
      If D_faktor < 0 Then Wait 10
      If Fusstaster = 0 Then Wait 10
      Stop Watchdog
'</Sicherheitstest 1>
'Maximalwerte für Datenschreiber/Notauserkennung auf 0 setzen
Regelzeit = 0
Regelzeitmax = 0
Mit_acc_nick_max = 0
Mit_acc_roll_max = 0
Pwm_rechts_max = 0
Pwm_links_max = 0
K_diff_min = 0
K_diff_max = 0
Richtung_links = 2                                                              'Leerlauf
Richtung_rechts = 2                                                             'Leerlauf
Pwm_links = 0
Pwm_rechts = 0
Pwm_gesamt = 0
Mit_acc_nick_s = 0
Lenkerstellung = 0
K_rechts = 0
K_links = 0
Seriell_err_l = 0
Seriell_err_r = 0
Ugrund = "----------------"                                                     'Einfach mal vorbelegen um zu sehen obs auch einen Notaus ohne Grund gibt.
On Timer0 Regel_interrupt
Config Timer0 = Timer , Prescale = 1024
Enable Timer0
Enable Interrupts
Waitms 100
Toggle Led3
Toggle Led4
Toggle Led5
Toggle Led6
Toggle Led7
Waitms 100
Toggle Led3
Toggle Led4
Toggle Led5
Toggle Led6
Toggle Led7
Waitms 100
Toggle Led3
Toggle Led4
Toggle Led5
Toggle Led6
Toggle Led7
Waitms 100
Toggle Led3
Toggle Led4
Toggle Led5
Toggle Led6
Toggle Led7
'<Sicherheitstest 2>
      Start Watchdog
      If Button1 = 0 Then Wait 10
      If Antriebaus = 0 Then Wait 10
      If P_faktor > 1 Then Wait 10
      If I_faktor > 1 Then Wait 10
      If D_faktor > 1 Then Wait 10
      If P_faktor < 0 Then Wait 10
      If I_faktor < 0 Then Wait 10
      If D_faktor < 0 Then Wait 10
      If Ugemessen < 1000 Then Wait 10
      If Ugemessen > 4700 Then Wait 10
      If Fusstaster = 0 Then Wait 10
      Stop Watchdog
'</Sicherheitstest 2>
If Ugemessen < 2400 Then                                                        'Sensoren ausschalten wenn am Ladegerät oder zu Testzwecken
    Sensor_aus = 1
Else
    Sensor_aus = 0
End If
While Ugemessen < 2400                                                          'Unterspannung oder am Ladegerät!
      Led3 = 1
      Led4 = 1
      Led5 = 1
      Led6 = 0
      Led7 = 1
Wend
'Prüfung ob Akku gerade geladen:
If Ugemessen > 3950 Then                                                        'Der Akku wurde gerade geladen
   Disable Interrupts                                                           'Interruts abschalten damit die folgenden Zeilen nicht gerade jetzt vom Interrupt unterbrochen werden.
   Waitms 50
   Summe_as_entladen = 0
   Summe_mah_entladen = 0
   Summe_as_laden = 0
   Summe_mah_laden = 0
   Enable Interrupts
End If
Summen_vorbelegen:
acc_roll = Getadc(5)                                                               'Roll
acc_nick = Getadc(7)                                                               'Nick
Gyro_roll = Getadc(2)                                                           'Roll
Gyro_nick = Getadc(4)                                                           'Nick
acc_roll = acc_roll * 100
acc_nick = acc_nick * 100
Sum_gyro_roll = Gyro_roll * Gyro_anz
Sum_gyro_nick = Gyro_nick * Gyro_anz
Gyro_roll_alt = Gyro_roll
Gyro_nick_alt = Gyro_nick
Mit_gyro_nick = Gyro_roll
Mit_gyro_nick = Gyro_nick
Sum_acc_roll = acc_roll * Acc_anz
Sum_acc_nick = acc_nick * Acc_anz
Waitms 100
Toggle Led3
Toggle Led4
Toggle Led5
Toggle Led6
Toggle Led7
Waitms 100
Toggle Led3
Toggle Led4
Toggle Led5
Toggle Led6
Toggle Led7
If Sig_gyro_nick < -3 Then Goto Summen_vorbelegen
If Sig_gyro_nick > 3 Then Goto Summen_vorbelegen
If Sig_gyro_roll < -3 Then Goto Summen_vorbelegen
If Sig_gyro_roll > 3 Then Goto Summen_vorbelegen
'<Sicherheitstest 3>
      Start Watchdog
      If Button1 = 0 Then Wait 10
      If Antriebaus = 0 Then Wait 10
      If P_faktor > 1 Then Wait 10
      If I_faktor > 1 Then Wait 10
      If D_faktor > 1 Then Wait 10
      If P_faktor < 0 Then Wait 10
      If I_faktor < 0 Then Wait 10
      If D_faktor < 0 Then Wait 10
      If Ugemessen < 1000 Then Wait 10
      If Ugemessen > 4700 Then Wait 10
      If Fusstaster = 0 Then Wait 10
      Stop Watchdog
'</Sicherheitstest 3>
'********************************************************************************************************************************************************
Do
'<Fusstaster abfragen>
If Fusscount = 0 Then
   If Antriebaus = 0 Then
      Antriebaus = 1
      Waitms 100                                                                'Damit der Regelinterrupt auch was davon mitbekommt
      Disable Interrupts
      Betriebsdatenschreiber
      Enable Interrupts
   End If
End If
'</Fusstaster abfragen>
'<Sicherheitstest Sensoren>
      If Antriebaus = 1 Then
         If Intcount <> Ltest Then
            Ltest = Intcount
            Sensor_ok = 0
            If Sensor_ok_count > 0 Then Decr Sensor_ok_count
            '1. Es darf keine nennenswerte Drehbewegung erkannt werden!
            If Sig_gyro_nick < -3 Then Sensor_ok_count = 50
            If Sig_gyro_nick > 3 Then Sensor_ok_count = 50
            If Sig_gyro_roll < -3 Then Sensor_ok_count = 50
            If Sig_gyro_roll > 3 Then Sensor_ok_count = 50
            '2. Die Werte dürfen nicht verrauscht sein
            Ldummy2 = Mit_acc_nick + 500
            If acc_nick > Ldummy2 Then Sensor_ok_count = 50
            Ldummy2 = Mit_acc_nick - 500
            If acc_nick < Ldummy2 Then Sensor_ok_count = 50
            Ldummy2 = Mit_acc_roll + 500
            If acc_roll > Ldummy2 Then Sensor_ok_count = 50
            Ldummy2 = Mit_acc_roll - 500
            If acc_roll < Ldummy2 Then Sensor_ok_count = 50
            Ldummy2 = Mit_gyro_nick + 5
            If Gyro_nick > Ldummy2 Then Sensor_ok_count = 50
            Ldummy2 = Mit_gyro_nick - 5
            If Gyro_nick < Ldummy2 Then Sensor_ok_count = 50
            Ldummy2 = Mit_gyro_roll + 5
            If Gyro_roll > Ldummy2 Then Sensor_ok_count = 50
            Ldummy2 = Mit_gyro_roll - 5
            If Gyro_roll < Ldummy2 Then Sensor_ok_count = 50
            '3. Die Werte müssen ungefähr in einer legalen Nullage sein.
            If Null_roll < 66150 Then Sensor_ok_count = 50
            If Null_roll > 69150 Then Sensor_ok_count = 50
            If null_nick < 63850 Then Sensor_ok_count = 50
            If null_nick > 66850 Then Sensor_ok_count = 50
            If Sensor_ok_count = 0 Then Sensor_ok = 1                           'Bedingungen während 50 Messungen (0.5s) eingehalten
         End If
      Else
         Sensor_ok = 0
      End If
 '</Sicherheitstest Sensoren>
'<Batterieanzeige>
      If Ugemessen > Akkufastleer Then                                          'alles unter 36V Leerlaufspannung ist LEER
               If Verbraucht > Akkukap Then
                  Akkukap = Verbraucht + 1                                      'Einfach mal annehmen daß noch mehr als angezeigt im Akku ist
                  Akkugrenzen
               End If
      End If
      'Mit Blinken wenn Gyros nicht bereit
      If Antriebaus = 1 Then
      If Ugemessen > Akkuleer Then                                              'alles unter 36V Leerlaufspannung ist LEER
               If Verbraucht > Akkukap Then
                  Akkukap = Verbraucht + 20                                     'Einfach mal annehmen daß noch mehr als angezeigt im Akku ist
                  Akkugrenzen
               End If
      End If
         If Sensor_ok = 1 Then
            If Verbraucht < Akku1 Then Led7 = 0 Else Led7 = 1
            If Verbraucht < Akku2 Then Led6 = 0 Else Led6 = 1
            If Verbraucht < Akku3 Then Led5 = 0 Else Led5 = 1
            If Verbraucht < Akku4 Then Led4 = 0 Else Led4 = 1
            If Verbraucht < Akku5 Then Led3 = 0 Else Led3 = 1
         Else
         If Frei = 1 Then                                                       'Schneller Blinken wenn Frei
            Blinkcount = Intcount
            If Blinkcount > 50 Then Blinkcount = Blinkcount - 50
         Else
            Blinkcount = Intcount / 2
         End If

            If Blinkcount > 40 Then
               If Verbraucht < Akku1 Then Led7 = 0 Else Led7 = 1
               If Verbraucht < Akku2 Then Led6 = 0 Else Led6 = 1
               If Verbraucht < Akku3 Then Led5 = 0 Else Led5 = 1
               If Verbraucht < Akku4 Then Led4 = 0 Else Led4 = 1
               If Verbraucht < Akku5 Then Led3 = 0 Else Led3 = 1
            Else
               Led7 = 1
               Led6 = 1
               Led5 = 1
               Led4 = 1
               Led3 = 1
            End If
         End If
      Else
         If Verbraucht < Akku1 Then Led7 = 0 Else Led7 = 1
         If Verbraucht < Akku2 Then Led6 = 0 Else Led6 = 1
         If Verbraucht < Akku3 Then Led5 = 0 Else Led5 = 1
         If Verbraucht < Akku4 Then Led4 = 0 Else Led4 = 1
         If Verbraucht < Akku5 Then Led3 = 0 Else Led3 = 1
      End If
'</Batterieanzeige>
'<Maximalwerte für Betriebsdatenschreiber>
      If Igemessen < Imin Then Imin = Igemessen                                 'Nur zur Information
      If Igemessen > Imax Then Imax = Igemessen                                 'Nur zur Information
      If Regelzeit > Regelzeitmax Then Regelzeitmax = Regelzeit
      If Mit_acc_nick > Mit_acc_nick_max Then Mit_acc_nick_max = Mit_acc_nick
      If Mit_acc_roll > Mit_acc_roll_max Then Mit_acc_roll_max = Mit_acc_roll
      If Mit_acc_nick < Mit_acc_nick_min Then Mit_acc_nick_min = Mit_acc_nick
      If Mit_acc_roll < Mit_acc_roll_min Then Mit_acc_roll_min = Mit_acc_roll
      If Pwm_links > Pwm_links_max Then Pwm_links_max = Pwm_links
      If Pwm_rechts > Pwm_rechts_max Then Pwm_rechts_max = Pwm_rechts
      If K_diff > K_diff_max Then K_diff_max = K_diff
      If K_diff < K_diff_min Then K_diff_min = K_diff
      If Ta > Tamax Then Tamax = Ta
'</Maximalwerte für Betriebsdatenschreiber>
If Intcount > 80 Then Led1 = 1 Else Led1 = 0                                    'Sekundenblinken
                                                                                'Daran lässt sich leicht erkennen ob
                                                                                'noch alles normal läuft.
Debounce Button1 , 0 , Onbutton1 , Sub
Loop
End
'**************************************************************************************************
'Ab hier kommen die Subroutinen und die Interrupts
'**************************************************************************************************
Sub Akkugrenzen
      'Grenzwerte für die Akku-LEDs berechnen
      Ldummy1 = Akkukap / 5
      Akku1 = Ldummy1
      Akku2 = 2 * Ldummy1
      Akku3 = 3 * Ldummy1
      Akku4 = 4 * Ldummy1
      Akku5 = 5 * Ldummy1
End Sub
'**************************************************************************************************
Sub Unfalldatenschreiber
Waitms 10
   Utext = Str(regelzeit)
   Utext = "Rz=" + Utext
   Logtext 0 , Utext
   Utext = Str(mit_acc_nick_max)
   Utext = "N max=" + Utext
   Logtext $10 , Utext
   Utext = Str(mit_acc_nick)
   Utext = "N=" + Utext
   Logtext $20 , Utext
   Utext = Str(mit_acc_nick_min)
   Utext = "N min=" + Utext
   Logtext $30 , Utext
   Utext = Str(mit_acc_roll_max)
   Utext = "R max=" + Utext
   Logtext $40 , Utext
   Utext = Str(mit_acc_roll)
   Utext = "R=" + Utext
   Logtext $50 , Utext
   Utext = Str(mit_acc_roll_min)
   Utext = "R min=" + Utext
   Logtext $60 , Utext
   Utext = Str(pwm_rechts)
   Utext = "PWM r=" + Utext
   Logtext $70 , Utext
   Utext = Str(pwm_links)
   Utext = "PWM l=" + Utext
   Logtext $80 , Utext
   Utext = Str(u_rechts)
   Utext = "U r=" + Utext
   Logtext $90 , Utext
   Utext = Str(u_links)
   Utext = "U l=" + Utext
   Logtext $a0 , Utext
   Utext = Str(ta)
   Utext = "Ta=" + Utext
   Logtext $b0 , Utext
   Utext = Str(seriell_err_l)
   Utext = Utext + "|"
   Utext = Utext + Str(seriell_err_r)
   Utext = "S =" + Utext
   Logtext $c0 , Utext
   Utext = Ugrund
   Logtext $d0 , Utext
End Sub
'**************************************************************************************************
Sub Betriebsdatenschreiber
'Ob die Wartezeiten beim EEprom schreiben nötig sind ist nicht sicher.
'Mit den Wartezeiten treten aber keine Fehler bei den Werten mehr auf.
'Möglich, daß dies auch durch eine andere Änderung bewirkt wurde.
Writeeeprom Max_v , $2f0
Writeeeprom Min_v , $2f2
Zentimeter = Int(zentimeter)                                                    'Die Millimeter sollen nicht geschrieben werden
Writeeeprom Summe_as_entladen , $300
Writeeeprom Summe_mah_entladen , $310
Writeeeprom Summe_as_laden , $320
Writeeeprom Summe_mah_laden , $330
Writeeeprom Akkukap , $340
Writeeeprom Kilometer , $350
Writeeeprom Meter , $360
Writeeeprom Zentimeter , $370
   If Werteneu = 1 Then                                                         'Nur wenn das Einstellklavier gesteckt ist.
      Writeeeprom P_faktor , $380
      Writeeeprom I_faktor , $384
      Writeeeprom D_faktor , $388
      Writeeeprom Mp_faktor , $38c
      Writeeeprom Mv_faktor , $390
      Writeeeprom Mpl_faktor , $394
      Writeeeprom Mvl_faktor , $398
      Werteneu = 0
   End If
   Utext = Str(pwm_rechts_max)
   Utext = "PWM r max=" + Utext
   Logtext $e0 , Utext
   Utext = Str(pwm_links_max)
   Utext = "PWM l max=" + Utext
   Logtext $f0 , Utext
   Utext = Str(ugemessen)
   Utext = "U=" + Utext
   Logtext $100 , Utext
   Utext = Str(imin)
   Utext = "Imin=" + Utext
   Logtext $110 , Utext
   Utext = Str(imax)
   Utext = "Imax=" + Utext
   Logtext $120 , Utext
   Utext = Str(summe_mah_entladen)
   Utext = "-mAh" + Utext
   Logtext $130 , Utext
   Utext = Str(summe_mah_laden)
   Utext = "+mAh=" + Utext
   Logtext $140 , Utext
   Utext = Str(verbraucht)
   Utext = "    =" + Utext
   Logtext $150 , Utext
   Utext = Str(akkukap)
   Utext = "Kap=" + Utext
   Logtext $160 , Utext
   Utext = Str(kilometer)
   Utext = "Km=" + Utext
   Logtext $170 , Utext
   Utext = Str(meter)
   Utext = "M=" + Utext
   Logtext $180 , Utext
   Utext = Str(zentimeter)
   Utext = "cm=" + Utext
   Logtext $190 , Utext
   Utext = Str(regelzeitmax)
   Utext = "Rz Max=" + Utext
   Logtext $1a0 , Utext
   Utext = Str(Null_roll)
   Utext = "Null_R=" + Utext
   Logtext $1b0 , Utext
   Utext = Str(null_nick)
   Utext = "null_N=" + Utext
   Logtext $1c0 , Utext
   Utext = Str(p_faktor)
   Utext = "P=" + Utext
   Logtext $1d0 , Utext
   Utext = Str(i_faktor)
   Utext = "I=" + Utext
   Logtext $1e0 , Utext
   Utext = Str(d_faktor)
   Utext = "D=" + Utext
   Logtext $1f0 , Utext
   Utext = Str(mp_faktor)
   Utext = "M=" + Utext
   Logtext $200 , Utext
   Utext = Str(k_diff_min)
   Utext = "K min=" + Utext
   Logtext $210 , Utext
   Utext = Str(k_diff_max)
   Utext = "K max=" + Utext
   Logtext $220 , Utext
   Utext = Str(seriell_err_l)
   Utext = "S l=" + Utext
   Logtext $230 , Utext
   Utext = Str(seriell_err_r)
   Utext = "S r=" + Utext
   Logtext $240 , Utext
   Utext = Str(tamax)
   Utext = "Ta_Max=" + Utext
   Logtext $250 , Utext
   Utext = Str(max_v)
   Utext = "V_Max=" + Utext
   Logtext $260 , Utext
   Utext = Str(min_v)
   Utext = "V_Min=" + Utext
   Logtext $270 , Utext
End Sub
'**************************************************************************************************
Sub Onbutton1
If Antriebaus = 1 Then
   If Ugemessen > Akkuleer Then                                                 'alles unter 36V Leerlaufspannung ist LEER
      If Sensor_ok = 1 Then                                                     'Prüfen ob Gyros noch driften
         Disable Interrupts                                                     'Interupts abschalten damit Fusscount nicht gerade jetzt decrementiert wird
         Regelzeit = 0
         Regelzeitmax = 0
         Mit_acc_nick_max = 0
         Mit_acc_roll_max = 0
         Mit_acc_nick_min = 0
         Mit_acc_roll_min = 0
         Pwm_rechts_max = 0
         Pwm_links_max = 0
         Seriell_err_r = 0
         Seriell_err_l = 0
         K_diff_min = 0
         K_diff_max = 0
         Antriebaus = 0
         Fusscount = 50                                                         '0.5 Sekunden zeit zum Aufsteigen
         Enable Interrupts
      Else
         If null_nick > 66850 And null_nick < 67450 Then
            If Bzeit = 500 Then
               Bstate = 1
               Bzeit = 0
            Else
               If Bstate = 1 Then
                  If Bzeit < 50 Then
                  Disable Interrupts
                  Utext = "                "
                  Logtext $00 , Utext
                  Logtext $10 , Utext
                  Logtext $20 , Utext
                  Logtext $30 , Utext
                  Logtext $40 , Utext
                  Logtext $50 , Utext
                  Logtext $60 , Utext
                  Logtext $70 , Utext
                  Logtext $80 , Utext
                  Logtext $90 , Utext
                  Logtext $a0 , Utext
                  Logtext $b0 , Utext
                  Logtext $c0 , Utext
                  Logtext $d0 , Utext
                  Logtext $e0 , Utext
                  Logtext $f0 , Utext
                  Logtext $100 , Utext
                  Logtext $110 , Utext
                  Logtext $120 , Utext
                  Logtext $130 , Utext
                  Logtext $140 , Utext
                  Logtext $150 , Utext
                  Logtext $160 , Utext
                  Logtext $170 , Utext
                  Logtext $180 , Utext
                  Logtext $190 , Utext
                  Logtext $1a0 , Utext
                  Logtext $1b0 , Utext
                  Logtext $1c0 , Utext
                  Logtext $1d0 , Utext
                  Logtext $1e0 , Utext
                  Logtext $1f0 , Utext
                  Logtext $200 , Utext
                  Logtext $210 , Utext
                  Logtext $220 , Utext
                  Logtext $230 , Utext
                  Logtext $240 , Utext
                  Logtext $250 , Utext
                  Logtext $260 , Utext
                  Summe_as_entladen = 0
                  Summe_mah_entladen = 0
                  Summe_as_laden = 0
                  Summe_mah_laden = 0
                  Writeeeprom Summe_as_entladen , $300
                  Writeeeprom Summe_mah_entladen , $310
                  Writeeeprom Summe_as_laden , $320
                  Writeeeprom Summe_mah_laden , $330
                  Frei = 0
                  For Bzeit = 1 To 10
                     Led3 = 1
                     Led4 = 1
                     Led5 = 1
                     Led6 = 1
                     Led7 = 1
                     Waitms 100
                     Led3 = 0
                     Led4 = 0
                     Led5 = 0
                     Led6 = 0
                     Led7 = 0
                     Waitms 100
                  Next
                  Bzeit = 500
                  Enable Interrupts
                  Waitms 100

                  End If
                  If Bzeit > 250 And Bzeit < 350 Then
                     Frei = 1
                     Bzeit = 500
                  Else
                     Frei = 0
                     Bzeit = 500

                  End If
               End If
            End If
         Else
         Frei = 0
         Bzeit = 500
         End If
      End If
   Else
      If Verbraucht < Akkukap Then                                              'Akku ist laut Spannung leer!
         Akkukap = Verbraucht * 0.95                                            'Einfach mal annehmen daß zuviel entnommen wurde
         Akkugrenzen
      End If
      Disable Interrupts
      Betriebsdatenschreiber
      Enable Interrupts
   End If
Else
   Antriebaus = 1
   Waitms 100
   Frei = 0
   Disable Interrupts
   Betriebsdatenschreiber
   Enable Interrupts
End If
End Sub
'**************************************************************************************************
Sub Logtext(adresse , Text)
   If Len(text) > 16 Then Text = Mid(text , 1 , 16)
   While Len(text) < 16
      Text = Text + " "
   Wend
   Writeeeprom Text , Adresse

End Sub
'**************************************************************************************************
Regel_interrupt:
'**************************************************************************************************
'Eigentlich ist das kein Typischer Interrupt. Der Interrupt wird hier nur verwendet um der
'Regelung einen 100Hz Takt zu geben. Normalerweise sollte kein Interrupt so lange sein.
'Dadurch werden andere Interrupts für diese Dauer blockiert.
'**************************************************************************************************
Timer0 = 61                                                                     'Sollte ca. 100Hz ergeben
Led2 = 1
If Fusscount > 0 Then Decr Fusscount
If Fusstaster = 0 Then Fusscount = 25                                           '0.25 Sekunden ohne Fusstaster
Bdummy1 = Udr
acc_roll = Getadc(5)                                                               'Roll
acc_nick = Getadc(7)                                                               'Nick
Gyro_roll = Getadc(2)                                                           'Roll
Gyro_nick = Getadc(4)                                                           'Nick
acc_roll = acc_roll * 100
acc_nick = acc_nick * 100
'<Mittellage ausgleichen>
   'Die Offsetwerte werden langsam an eine früher ermittelte Waagerechte Positoion angeglichen.
   Null_roll = Null_roll * 99
   null_nick = null_nick * 99
   Null_roll = Null_roll + Offset_roll                                                'Mittellage
   null_nick = null_nick + Offset_nick                                                'Mittellage
   Null_roll = Null_roll / 100
   null_nick = null_nick / 100
'</Mittellage ausgleichen>
'Offsetwerte der Beschleunigungssensoren sofort abziehen
'Diese werden nicht laufend ermittelt
acc_roll = acc_roll - Null_roll
acc_nick = acc_nick - null_nick
'<Geschwindikgeitsbegrenzung>
      'Bei der Geschwindigkeitsbegrenzung wird einfach die Nullage der Plattform verändert.
      'Beim Vorwärtsbremsen wird nach hinten geneigt.
      'Beim Rückwärtsbremsen nach vorne.
      'Höchstgeschwindigkeit im Gesetzlichen Rahmen unter 6Km/h (Ausserhalb der KFZ Zulassungsordnung)
      Vbrems = 0
      Lbrems = 0
      If Frei = 0 Then
         If Kmh > Vmax Then Vbrems = Vmax - Kmh
         If Kmh < Vmaxr Then Vbrems = Vmaxr - Kmh
      End If
      'Lastabhängige Bremse
      'Es muss immer genug Reserve sein um noch zu Bremsen oder zu Balancieren
      Ldummyi = Int(pwm_gesamt)
      If Ldummyi > Spwm Then Lbrems = Spwm - Ldummyi
      If Ldummyi < Spwmr Then Lbrems = Spwmr - Ldummyi
      Brems = Lbrems + Vbrems
      Brems = Brems * 50
      'Nicht zu heftig Bremsen
      If Brems < -2000 Then Brems = -2000
      If Brems > 2000 Then Brems = 2000
      acc_nick = acc_nick + Brems
'</Geschwindikgeitsbegrenzung>
'<Signalaufbereitung der Sensorsignale>
      K_diff = K_rechts - K_links
      K_sum = K_sum * 0.84
      K_sum = K_sum + K_diff
      'Gleitender Mittelwert der Gyros berechnen um damit den Offset bei Stillstand
      'zu haben. Dieser ist leider Temperaturabhängig.
      Sum_gyro_roll = Sum_gyro_roll - Mit_gyro_roll
      Sum_gyro_roll = Sum_gyro_roll + Gyro_roll
      Mit_gyro_roll = Sum_gyro_roll / Gyro_anz
      Sig_gyro_roll = Gyro_roll - Mit_gyro_roll
      Korr_acc_roll = Sig_gyro_roll * Gyro_faktor
      Sum_acc_roll = Sum_acc_roll - Mit_acc_roll
      Sum_acc_roll = Sum_acc_roll + acc_roll
      Sum_acc_roll = Sum_acc_roll + Korr_acc_roll                                        'Gyrowert hinzu
      Mit_acc_roll = Sum_acc_roll / Acc_anz
'************************************************************************************************************************************************************************************
      Sdummy1 = Mit_acc_roll * K_sum                                            'Das soll ausgleichen, daß die Gyros bei gekippten Sensoren eine Drehung um die falsche Achse 
                                                                                'registrieren.
      Sdummy1 = Sdummy1 / 2000                                                  'Die Sensorplatine muss dazu absolut Waagerecht montiert sein.
      Lagefehler = Int(sdummy1)
      Gyro_nick = Gyro_nick - Lagefehler                                        'Das soll den Fehler ausgleichen, der durch die geänderte Lage des Gyros auftritt.
 '************************************************************************************************************************************************************************************
      Sum_gyro_nick = Sum_gyro_nick - Mit_gyro_nick
      Sum_gyro_nick = Sum_gyro_nick + Gyro_nick
      Mit_gyro_nick = Sum_gyro_nick / Gyro_anz
      Sig_gyro_nick = Gyro_nick - Mit_gyro_nick
      Korr_acc_nick = Sig_gyro_nick * Gyro_faktor
      Sum_acc_nick = Sum_acc_nick - Mit_acc_nick
      Sum_acc_nick = Sum_acc_nick + acc_nick
      Sum_acc_nick = Sum_acc_nick - Korr_acc_nick                               'Gyrowert hinzu
      Mit_acc_nick = Sum_acc_nick / Acc_anz
'</Signalaufbereitung der Sensorsignale>
'<PID-Regler>
   Stellwert = 0
   '<P-Regler>
      Sdummy1 = Mit_acc_nick
      Sdummy1 = Sdummy1 * P_faktor
      Stellwert = Stellwert - Sdummy1
   '</P-Regler>
   '<I-Regler>
      Mit_acc_nick_s = Mit_acc_nick_s + Mit_acc_nick
      'Überlauf/unsinnige Werte verhindern
      If Mit_acc_nick_s > Max_mit_acc_nick_s Then Mit_acc_nick_s = Max_mit_acc_nick_s
      If Mit_acc_nick_s < Min_mit_acc_nick_s Then Mit_acc_nick_s = Min_mit_acc_nick_s
      Sdummy1 = Mit_acc_nick_s
      Sdummy1 = Sdummy1 * I_faktor
      Stellwert = Stellwert - Sdummy1
   '</I-Regler>
   '<D-Regeler>
      Sdummy1 = Sig_gyro_nick
      Sdummy1 = Sdummy1 * D_faktor
      Stellwert = Stellwert + Sdummy1
   '</D-Regler>
   '<Anpassungsfaktoren berechnen>
   K_sum_anpassung = K_rechts + K_links
   K_sum_anpassung = Abs(k_sum_anpassung)
   Mp_anpassung = Pwm_gesamt * Mp_faktor
   Mp_anpassung = Abs(mp_anpassung)
   Mp_anpassung = Mp_anpassung + 0.95
   Mv_anpassung = K_sum_anpassung * Mv_faktor
   Mv_anpassung = Abs(mv_anpassung)
   Mv_anpassung = Mv_anpassung + 1
   Mpl_anpassung = Pwm_gesamt * Mpl_faktor
   Mpl_anpassung = Abs(mpl_anpassung)
   Mpl_anpassung = Mpl_anpassung + 1
   Mvl_anpassung = K_sum_anpassung * Mvl_faktor
   Mvl_anpassung = Abs(mvl_anpassung)
   Mvl_anpassung = Mvl_anpassung + 1
   '</Anpassungsfaktoren berechnen>
   '<Verstärkte Regelung bei hoher Leistung>
   Stellwert = Stellwert * Mp_anpassung
   Stellwert = Stellwert * Mv_anpassung
   '<Verstärkte Regelung bei hoher Leistung>
   '<Abgeschwächte Regelung in Stand>
   If K_sum_anpassung < 8 Then Stellwert = Stellwert * 0.8
   If K_sum_anpassung < 6 Then Stellwert = Stellwert * 0.8
   If K_sum_anpassung < 4 Then Stellwert = Stellwert * 0.8
   '</Abgeschwächte Regelung in Stand>
   Pwm_gesamt = Pwm_gesamt + Stellwert
'</PID-Regler>
'<Bremsvibrator einmischen>
   'Der Bremsvibrator signalisiert dem Fahrer, daß wegen irgend einem Grenzwert gebremst wird.
   'Das kann V-Max oder P-Max sein.
    If Brems <> 0 Then
      Bdummy1 = Intcount / 2
      Bdummy1 = Bdummy1 Mod 2
      If Bdummy1 = 0 Then
         Pwm_gesamt = Pwm_gesamt + 5
      Else
         Pwm_gesamt = Pwm_gesamt - 5
      End If
    End If
'<Bremsvibrator einmischen>
'<Lenkung>
   Lenkvorgabe = Mit_acc_roll / 80
   Sdummy1 = Abs(lenkvorgabe)
   If Sdummy1 <= 6 Then
      Sdummy1 = Sdummy1 / 12
      Lenkerstellung = Lenkvorgabe * Sdummy1
   Else
      If Lenkvorgabe < 0 Then
         Lenkerstellung = Lenkvorgabe + 3
      Else
         Lenkerstellung = Lenkvorgabe - 3
      End If
   End If
   'Lenkerstellung = Lenkerstellung * MP_anpassung                             'Dabei kann sich die Lenkung extrem aufschaukeln.
   'Lenkfehler berechnen und zum Stellwert hinzu.
   Lenkfehler = Lenkerstellung + K_sum
   Lenkfehler = Lenkfehler * Mpl_anpassung
   Lenkfehler = Lenkfehler * Mvl_anpassung
   Lenkstellwert = Lenkerstellung + Lenkfehler
   If Lenkstellwert > Max_lenk Then Lenkstellwert = Max_lenk
   If Lenkstellwert < Min_lenk Then Lenkstellwert = Min_lenk
   Pwm_lenk_links = Pwm_gesamt + Lenkstellwert
   Pwm_lenk_rechts = Pwm_gesamt - Lenkstellwert
   If Pwm_lenk_links > 0 Then Richtung_links = 1 Else Richtung_links = 0
   If Pwm_lenk_rechts > 0 Then Richtung_rechts = 1 Else Richtung_rechts = 0
   Pwm_lenk_links = Abs(pwm_lenk_links)
   Pwm_lenk_rechts = Abs(pwm_lenk_rechts)
   If Pwm_lenk_links > Maxpwm Then Pwm_lenk_links = Maxpwm
   If Pwm_lenk_rechts > Maxpwm Then Pwm_lenk_rechts = Maxpwm
   If Pwm_lenk_links < Minpwm Then Pwm_lenk_links = Minpwm
   If Pwm_lenk_rechts < Minpwm Then Pwm_lenk_rechts = Minpwm
   Pwm_links = Pwm_lenk_links
   Pwm_rechts = Pwm_lenk_rechts
'</Lenkung>
If Antriebaus = 1 Then
   If Notaus = 1 Then
      'Unfalldatenschreiber: *********************************************************
      'Dauert zwar Lange, macht aber nix. Bei Notaus muss nix geregelt werden.
      Unfalldatenschreiber
      Betriebsdatenschreiber
      Notaus = 0
      '*******************************************************************************
   End If
'Auf 0 setzen was sonst geregelt wird
   Richtung_links = 2                                                           'Leerlauf
   Richtung_rechts = 2                                                          'Leerlauf
   Pwm_links = 0
   Pwm_rechts = 0
   Pwm_gesamt = 0
   Mit_acc_nick_s = 0
   Lenkerstellung = 0
   K_rechts = 0
   K_links = 0
   'Waagerechte Positoion setzen wenn Zustand Antriebaus:
   Wdummy1 = Getadc(5)
   Null_roll = Wdummy1 * 100
   Wdummy1 = Getadc(7)
   null_nick = Wdummy1 * 100
   'Mittelwert der Beschleunigungssensoren auf aktuellen Wert stellen
   'Der Mittelwert braucht sonst zu lange bis
   'er sich eingeregelt hat
   'Sum_acc_roll = Acc_roll * Acc_anz
   'Sum_acc_nick = Acc_nick * Acc_anz
   If Intcount = 0 Then T30 = 1                                                 'Regelparameter vom Tacho anfordern
   If Intcount = 50 Then T30 = 1                                                'Regelparameter vom Tacho anfordern
End If
'*******************************************************************************
   If T30 = 1 Then                                                              'Regelparameter vom Tacho anfordern
      K_links = 0
      K_rechts = 0
      Zeiger_meldung = 1
      Bdummy1 = Udr
      T30 = 0                                                                   'Damit keine alte Rückmeldung ausgewertet wird
      Timeout = Timer0
      Timeout = Timeout + 100
      Print Chr(255) ; Chr(0) ; Chr(0) ; Chr(255) ; Chr(13)
      While Zeiger_meldung < 31
         If Ucsr0a.rxc0 = 1 Then                                                'Warten bis ein Byte eingetroffen ist
            Tachomeldung(zeiger_meldung) = Udr
            Incr Zeiger_meldung
            Timeout = Timer0
            Timeout = Timeout + 20
         End If
         If Timer0 > Timeout Then
         T30 = 0
         Exit While                                                             'Timeout
         End If
      Wend
      If T30 = 13 Then
         $asm
            lds R24,{t1}
            LDs R25,{t2}
            add R24,R25
            LDs R25,{t3}
            add R24,R25
            LDs R25,{t4}
            add R24,R25
            LDs R25,{t5}
            add R24,R25
            LDs R25,{t6}
            add R24,R25
            LDs R25,{t7}
            add R24,R25
            LDs R25,{t8}
            add R24,R25
            LDs R25,{t9}
            add R24,R25
            LDs R25,{t10}
            add R24,R25
            LDs R25,{t11}
            add R24,R25
            LDs R25,{t12}
            add R24,R25
            LDs R25,{t13}
            add R24,R25
            LDs R25,{t14}
            add R24,R25
            LDs R25,{t15}
            add R24,R25
            LDs R25,{t16}
            add R24,R25
            LDs R25,{t17}
            add R24,R25
            LDs R25,{t18}
            add R24,R25
            LDs R25,{t19}
            add R24,R25
            LDs R25,{t20}
            add R24,R25
            LDs R25,{t21}
            add R24,R25
            LDs R25,{t22}
            add R24,R25
            LDs R25,{t23}
            add R24,R25
            LDs R25,{t24}
            add R24,R25
            LDs R25,{t25}
            add R24,R25
            LDs R25,{t26}
            add R24,R25
            LDs R25,{t27}
            add R24,R25
            LDs R25,{t28}
            add R24,R25
            STS {Bdummy1},R24
         $end Asm

         If Bdummy1 = T29 Then
            P_faktor = P_tacho
            I_faktor = I_tacho
            D_faktor = D_tacho
            Mp_faktor = Mp_tacho
            Mv_faktor = Mv_tacho
            Mpl_faktor = Mpl_tacho
            Mvl_faktor = Mvl_tacho
            Werteneu = 1
         End If
      End If
   Else
   'Errechnete Geschwindigkeiten an Motoren Senden:
   'Rechter Motor:
   Checksum_rechts = 82
   Checksum_rechts = Checksum_rechts + Richtung_rechts
   Checksum_rechts = Checksum_rechts + Pwm_rechts
   'Geschwindigkeit an Motorregler senden
   Zeiger_meldung = 1
   Bdummy1 = Udr                                                                'Puffer leeren
   'Print Chr(82) ; Chr(richtung_rechts) ; Chr(pwm_rechts) ; Chr(checksum_rechts) ; Chr(13);
   'In Assembler geht das so:
   $asm
   Ldi R24 , 82
   STS     udr,R24
   Warten1:
   LDS     R24,UCSR0A
   BST     R24,5
   Brtc warten1
   LDS     R24,{Richtung_rechts}
   STS     udr,R24
   Warten2:
   LDS     R24,UCSR0A
   BST     R24,5
   Brtc warten2
   lds     R24,{Pwm_rechts}
   STS     udr,R24
   Warten3:
   LDS     R24,UCSR0A
   BST     R24,5
   Brtc warten3
   LDS     R24,{Checksum_rechts}
   STS     udr,R24
   Warten4:
   LDS     R24,UCSR0A
   BST     R24,5
   Brtc warten4
   LDI     R24,13
   STS     udr,R24
   Warten5:
   LDS     R24,UCSR0A
   BST     R24,5
   Brtc warten5
   $end Asm
   Motormeldung(8) = 0                                                          'Damit keine alte Rückmeldung ausgewertet wird
   Timeout = Timer0
   Timeout = Timeout + 80
   While Zeiger_meldung < 9
      If Ucsr0a.rxc0 = 1 Then                                                   'Warten bis ein Byte eingetroffen ist
         Motormeldung(zeiger_meldung) = Udr
         Incr Zeiger_meldung
      End If
      If Timer0 > Timeout Then
      Ende = 0
      Exit While                                                                'Timeout
      End If
   Wend
   $asm
               lds R24,{M1}
               LDs R25,{M2}
               ADD R24,R25
               LDs R25,{M3}
               ADD R24,R25
               LDs R25,{M4}
               ADD R24,R25
               LDs R25,{M5}
               ADD R24,R25
               LDs R25,{M6}
               ADD R24,R25
               STS {Bdummy1},R24
   $end Asm
   If Bdummy1 <> Checksumme Then Ende = 0
   If Ende = 13 Then
      U_rechts = U
      I_rechts = I
      K_rechts = K
   Else
      Incr Seriell_err_r
      S_err = 1
   End If
   'Linker Motor:
   Checksum_links = 76
   Checksum_links = Checksum_links + Richtung_links
   Checksum_links = Checksum_links + Pwm_links
   Zeiger_meldung = 1
   Bdummy1 = Udr                                                                'Puffer leeren
   'Print Chr(76) ; Chr(richtung_links) ; Chr(pwm_links) ; Chr(checksum_links) ; Chr(13);
   'In Assembler geht das so:
   $asm
   Ldi     R24 , 76
   STS     udr,R24
   Warten6:
   LDS     R24,UCSR0A
   BST     R24,5
   Brtc warten6
   LDS     R24,{Richtung_links}
   STS     udr,R24
   Warten7:
   LDS     R24,UCSR0A
   BST     R24,5
   Brtc warten7
   lds     R24,{Pwm_links}
   STS     udr,R24
   Warten8:
   LDS     R24,UCSR0A
   BST     R24,5
   Brtc warten8
   LDS     R24,{Checksum_links}
   STS     udr,R24
   Warten9:
   LDS     R24,UCSR0A
   BST     R24,5
   Brtc warten9
   LDI     R24,13
   STS     udr,R24
   Warten10:
   LDS     R24,UCSR0A
   BST     R24,5
   Brtc warten10
   $end Asm
   Motormeldung(8) = 0                                                          'Damit keine alte Rückmeldung ausgewertet wird
   Timeout = Timer0
   Timeout = Timeout + 80
   While Zeiger_meldung < 9
      If Ucsr0a.rxc0 = 1 Then                                                   'Warten bis ein Byte eingetroffen ist
         Motormeldung(zeiger_meldung) = Udr
         Incr Zeiger_meldung
      End If
      If Timer0 > Timeout Then
      Ende = 0
      Exit While                                                                'Timeout
      End If
   Wend
   $asm
               lds R24,{M1}
               LDs R25,{M2}
               ADD R24,R25
               LDs R25,{M3}
               ADD R24,R25
               LDs R25,{M4}
               ADD R24,R25
               LDs R25,{M5}
               ADD R24,R25
               LDs R25,{M6}
               ADD R24,R25
               STS {Bdummy1},R24
   $end Asm
   If Bdummy1 <> Checksumme Then Ende = 0
   If Ende = 13 Then
      U_links = U
      I_links = I
      K_links = K
   Else
      Incr Seriell_err_l
      S_err = 1
   End If
If S_err = 1 Then                                                               'Sicherheitshalber, damit der Lenkausgleich nicht überreagiert.
  K_links = 0
  K_rechts = 0
  S_err = 0
End If
End If
 If Antriebaus = 0 Then
'<Notauskriterien ermitteln>
      If Mit_acc_nick > -300 Then
         If Mit_acc_nick < 300 Then
            Regelzeit = 0
         End If
      End If
   Incr Regelzeit
'</Notauskriterien ermitteln>
'<Notauskriterien abprüfen>
   If Regelzeit > 250 Then                                                      'Wenn nach 2,5s waagerechte Lage nicht erreicht
      Notaus = 1
      Antriebaus = 1
      Ugrund = Str(regelzeit)
      Ugrund = "*Rz " + Ugrund
   End If
   Ldummyi = 4000 + Brems
   If Mit_acc_nick > Ldummyi Then                                                  'zu weit nach hinten gekippt
      Notaus = 1
      Antriebaus = 1
      Ugrund = Str(mit_acc_nick)
      Ugrund = "*Nick " + Ugrund
   End If
   Ldummyi = -4000 + Brems
   If Mit_acc_nick < Ldummyi Then                                                  'zu weit nach vorn gekippt
      Notaus = 1
      Antriebaus = 1
      Ugrund = Str(mit_acc_nick)
      Ugrund = "*Nick " + Ugrund
   End If
   If Mit_acc_roll > 5000 Then                                                     'Lenker zu weit
      Notaus = 1
      Antriebaus = 1
      Ugrund = Str(mit_acc_roll)
      Ugrund = "*Roll " + Ugrund
   End If
   If Mit_acc_roll < -5000 Then                                                    'Lenker zu weit
      Notaus = 1
      Antriebaus = 1
       Ugrund = Str(mit_acc_roll)
      Ugrund = "*Roll " + Ugrund
   End If
   If K_diff > 10 Then                                                          'Plattform rotiert zu schnell
      Notaus = 1
      Antriebaus = 1
      Ugrund = Str(k_diff)
      Ugrund = "*Gier " + Ugrund
   End If
   If K_diff < -10 Then                                                         'Plattform rotiert zu schnell
      Notaus = 1
      Antriebaus = 1
      Ugrund = Str(k_diff)
      Ugrund = "*Gier " + Ugrund
   End If
'</Notauskriterien abprüfen>
End If
'<Akkuspannung berechnen>
   Ugemessen = U_links + U_rechts
   Shift Ugemessen , Right , 1 , Signed
'</Akkuspannung berechnen>
'<Gesamtstrom berechnen>
   Igemessen = I_links + I_rechts
'</Gesamtstrom berechnen>
'<verbrauchte As berechnen>
   If Igemessen > 0 Then
      Summe_as_entladen = Summe_as_entladen + Igemessen                         'Verbrauchte
   End If
   If Igemessen < 0 Then
      Summe_as_laden = Summe_as_laden - Igemessen                               'Rückgespeiste
   End If
   'mAh aus den As rausholen
   'um überläufe zu verhindern
   'Die 360000 kommen weil:
   'Igemessen in mA -> Faktor 1
   '100 Messungen pro Sekunde -> mal 100
   '3600 mAmperesekunden=1 mAh -> also nochmal mal 3600
   If Summe_as_entladen > 360000 Then
      Summe_mah_entladen = Summe_mah_entladen + 1
      Summe_as_entladen = Summe_as_entladen - 360000
   End If
   If Summe_as_laden > 360000 Then                                              'Das kann passieren wenns den Berg runter geht
      Summe_mah_laden = Summe_mah_laden + 1
      Summe_as_laden = Summe_as_laden - 360000
   End If
      Erzeugt = Summe_mah_laden
      Verbraucht = Summe_mah_entladen
      Erzeugt = Erzeugt * 0.7                                                   'Das ist der Akku-Wirkungsgrad 0.7 (0.85) bezogen auf den Stom
      Verbraucht = Verbraucht - Erzeugt
      Akkuinhalt = Akkukap
      Akkuverbrauch = Verbraucht
      Akkuprozent = Akkuverbrauch / Akkuinhalt
      Akkuprozent = Akkuprozent * 1000
      Akkuprozent = Int(akkuprozent)
      Akkustand = Akkuprozent
'</verbrauchte As berechnen>
'<Strecke berechnen>
   Kx = K_links + K_rechts
   Kx = Kx / 2
   Strecke = Kx * Di
   Zentimeter = Zentimeter + Strecke
   If Zentimeter >= 100 Then
      Meter = Meter + 1
      Zentimeter = Zentimeter - 100
   End If
   If Zentimeter < 0 Then
      Meter = Meter - 1
      Zentimeter = Zentimeter + 100
   End If
   If Meter >= 1000 Then
      Kilometer = Kilometer + 1
      Meter = Meter - 1000
   End If
   If Meter < 0 Then
      Kilometer = Kilometer - 1
      Meter = Meter + 1000
   End If
'</Strecke berechnen>
'<Geschwindigkeit berechnen>
Kv = Kv + Kx
Kmh = Kv * Kmhfaktor
Kv = Kv * 0.75                                                                  'Bei Änderung auch den kmhfaktor neu berechnen
If Kmh > Max_v Then Max_v = Kmh
If Kmh < Min_v Then Min_v = Kmh
'</Geschwindigkeit berechnen>
If Bzeit < 500 Then Incr Bzeit
Incr Intcount
Incr Tachocount
Select Case Tachocount
'*******************************************************************************
'Alle möglichen Werte werden an den Tacho geschickt. Manche für die Funktion
'des Tachos, andere um die Steuerung zu optimieren.
'*******************************************************************************
Case 0                                                                          'U ausgeben
     B(1) = 1
     U = Ugemessen
     'U = Sig_Gyro_Nick
     B(2) = Motormeldung(1)
     B(3) = Motormeldung(2)
     B(5) = 1
Case 1                                                                          'I ausgeben
     B(1) = 2
     I = Igemessen / 10
     B(2) = Motormeldung(3)
     B(3) = Motormeldung(4)
     B(5) = 1
Case 2                                                                          'Km/h
     B(1) = 3
     B(2) = Kmhmeldung(1)
     B(3) = Kmhmeldung(2)
     B(5) = 1
Case 3                                                                          'Akkuprozente
     B(1) = 5
     B(2) = Akkumeldung(1)
     B(3) = Akkumeldung(2)
     B(5) = 1
Case 4                                                                          'Meter
     B(1) = 7
     B(2) = Metermeldung(1)
     B(3) = Metermeldung(2)
     B(5) = 1
Case 5                                                                          'Kilometer
     B(1) = 6
     B(2) = Kilometermeldung(1)
     B(3) = Kilometermeldung(2)
     B(5) = 1
Case 6
      If Antriebaus = 0 Then
         Maccroll = Mit_acc_roll
         B(1) = 8
         B(2) = Maccroll1
         B(3) = Maccroll2
         B(4) = 1
      Else
         Nullroll = Null_roll
         B(1) = 12
         B(2) = Nullroll1
         B(3) = Nullroll2
         B(4) = 1
      End If
Case 7
      If Antriebaus = 0 Then
         B(1) = 9
         B(2) = Maccroll3
         B(3) = Maccroll4
         B(4) = 1
      Else
         B(1) = 13
         B(2) = Nullroll3
         B(3) = Nullroll4
         B(4) = 1
      End If
Case 8
      B(1) = 4
      B(2) = Antriebaus
      B(3) = Ta
      B(4) = 1
Case 9
      If Antriebaus = 0 Then
         maccnick = Mit_acc_nick
         B(1) = 10
         B(2) = maccnick1
         B(3) = maccnick2
         B(4) = 1
      Else
         nullnick = null_nick
         B(1) = 14
         B(2) = nullnick1
         B(3) = nullnick2
         B(4) = 1
      End If
Case 10
      If Antriebaus = 0 Then
         B(1) = 11
         B(2) = maccnick3
         B(3) = maccnick4
         B(4) = 1
      Else
         B(1) = 15
         B(2) = nullnick3
         B(3) = nullnick4
         B(4) = 1
      End If
Case 11
      Sw = Stellwert
      B(1) = 16
      B(2) = Sw1
      B(3) = Sw2
      B(4) = 1
Case 12
      B(1) = 17
      B(2) = Sw3
      B(3) = Sw4
      B(4) = 1
Case 13
      Gyronick = Sig_gyro_nick
      B(1) = 18
      B(2) = Gyronick1
      B(3) = Gyronick2
      B(4) = 1
Case 14
      B(1) = 19
      B(2) = Gyronick3
      B(3) = Gyronick4
      B(4) = 1
      'Tachocount = 7  'Wenn die wichtigsten Parameter schnell übermittelt werden sollen.
Case 15
      Kksum = K_sum
      B(1) = 20
      B(2) = Kksum1
      B(3) = Kksum2
      B(4) = 1
Case 16
      B(1) = 21
      B(2) = Kksum3
      B(3) = Kksum4
      B(4) = 1
If Antriebaus = 0 Then Tachocount = 255                                         'Gleich zu 0 überlaufen lassen
Case 17
      B(1) = 22
      B(2) = Pf1
      B(3) = Pf2
      B(4) = 1
Case 18
      B(1) = 23
      B(2) = Pf3
      B(3) = Pf4
      B(4) = 1
Case 19
      B(1) = 24
      B(2) = If1
      B(3) = If2
      B(4) = 1
Case 20
      B(1) = 25
      B(2) = If3
      B(3) = If4
      B(4) = 1
Case 21
      B(1) = 26
      B(2) = Df1
      B(3) = Df2
      B(4) = 1
Case 22
      B(1) = 27
      B(2) = Df3
      B(3) = Df4
      B(4) = 1
Case 23
      B(1) = 28
      B(2) = Mpf1
      B(3) = Mpf2
      B(4) = 1
Case 24
      B(1) = 29
      B(2) = Mpf3
      B(3) = Mpf4
      B(4) = 1
Case 25
      B(1) = 30
      B(2) = Mvf1
      B(3) = Mvf2
      B(4) = 1
Case 26
      B(1) = 31
      B(2) = Mvf3
      B(3) = Mvf4
      B(4) = 1
Case 27
      B(1) = 32
      B(2) = Mplf1
      B(3) = Mplf2
      B(4) = 1
Case 28
      B(1) = 33
      B(2) = Mplf3
      B(3) = Mplf4
      B(4) = 1
Case 29
      B(1) = 34
      B(2) = Mvlf1
      B(3) = Mvlf2
      B(4) = 1
Case 30
      B(1) = 35
      B(2) = Mvlf3
      B(3) = Mvlf4
      B(4) = 1
Tachocount = 255
Case Else
B(5) = 0                                                                        'Nix ausgeben
End Select
If Intcount = 99 Then Intcount = 255
If B(5) = 1 Then
   'Checksumme berechnen
   B(4) = B(1)
   B(4) = B(4) + B(2)
   B(4) = B(4) + B(3)
'Print Chr(b(1)) ; Chr(b(2)) ; Chr(b(3)) ; Chr(b(4)) ; Chr(13);
'Print in Assembler:
$asm
   LDS     R24,285                                                              'b(1)
   STS     udr,R24
   Warten11:
   LDS     R24,UCSR0A
   BST     R24,5
   Brtc warten11
   LDS     R24,286                                                              'b(2)
   STS     udr,R24
   Warten12:
   LDS     R24,UCSR0A
   BST     R24,5
   Brtc warten12
   lds     R24,287                                                              'B(3)
   STS     udr,R24
   Warten13:
   LDS     R24,UCSR0A
   BST     R24,5
   Brtc warten13
   LDS     R24,288                                                              'B(4)
   STS     udr,R24
   Warten14:
   LDS     R24,UCSR0A
   BST     R24,5
   Brtc warten14
   LDI     R24,13
   STS     udr,R24
   Warten15:
   LDS     R24,UCSR0A
   BST     R24,5
   Brtc warten15
$end Asm
   Ta = Timer0                                                                  'Um festzustellen ob der Interrupt zu lange dauert
End If
Led2 = 0
Return

Tacho

Akkuladestecker

Software

Mechanik

Motorumbau


LiFePO4 Speicher Test