'---------------------------------------------------------------------------
'  _____ _     _   _              _ _ _ _           _ _
' |   __| |___| |_| |_ ___ ___   | | | | |_ ___ ___| |_|___
' |   __| | -_| '_|  _| . |  _|  | | | |   | -_| -_| | | -_|
' |_____|_|___|_,_|_| |___|_|    |_____|_|_|___|___|_|_|___|
'
' (c)2009 Elektor International Media B.V.
'---------------------------------------------------------------------------
'Version: WOF104 - Elektor Wheelie Open Firmware 1.04
'---------------------------------------------------------------------------
'Purpose:
'  Elektor-Wheelie motor control source code
'---------------------------------------------------------------------------
'Description:
'  On power on the LEDs are flashing in the order green-yellow-red
'  If initialization fails the red LED is blinking and the Wheelie is blocked.
'  Blinking 2 times = gyro failure, 3 times = adxl failure.
'  If initialization has been successful the 3 LEDs show the battery status:
'  green = battery full, yellow = battery empty and red = battery dead.
'---------------------------------------------------------------------------'
'Older sources:
'  FW 2.6: Original Elekor firmware version 2.6 by Chris Krohne 2009
'  based on the AT3329 Project of Reinhold Pieper
'---------------------------------------------------------------------------'
'Changes Log:
'  WOF100:
'     Starting point for improvements from the community. Equivalent to beta.4.
'  WOF101:
'     foot switch: security brake before power down
'     Bug in WOF100 removed
'  WOF102:
'     foot switch: timeout inserted
'     faster Code in Algo, thanks Thomas Scherer
'     Telemetry added again
'     Speedsensors and I²C added
'     Testmode Switch
'     first Steps for steering Control
'  WOF103beta:
'     adxl and gyro * 10 to prevent rounding errors
'  WOF103:
'     Finetuning
'  WOF104:
'     Batteryvalues corrected. The resistor R10 must be changed from 68k to 100k!!!!
'     more Staus and Errorinfos for Telemetry and Display
'     Voltage to Telemetry and Display
'     Bug in Speed measuring fixed
'     Steering depends on Speed now
'     Steering is corrected by the speeddifference of the wheels
'     Wheelie II is supported now
'     First Zzaag tests
'
'---------------------------------------------------------------------------'
'ToDo:
'  Improvement of steering control (Zeroposition depends on geocentre)
'  Speed limit (6km/h-Limit to switch on and off)
'  emergency brake at a fall (analyse Switch, Tilt and Gyro)
'---------------------------------------------------------------------------'
'Contact:
'  Please post ideas, improvements and code into this thread in the Elektor forum:
'  http://www.elektor.de/forum/foren-ubersicht/foren-zu-elektor-projekten/elektorwheelie.996934.lynkx
'  Günter Gerold: tv@gerold-online.de
'---------------------------------------------------------------------------

$crystal = 16000000
$baud = 57600
$regfile = "m32def.dat"
$hwstack = 50                                               'Hardware stack
$swstack = 50                                               'Software stack
$framesize = 50                                             'Frame space

'---------------------------------------------------------------------------
'Switches
'---------------------------------------------------------------------------
'#####################
Const Test_enable = 1                                       'Turn on and off the Testmode here! (0 or 1)
'#####################
'#####################
Const I2c_enable = 0                                        'Turn the I²C-Geartoothsensors on and off here! (0 or 1)
'#####################
'#####################
Const Rocker_enable = 0                                     'Turn the X-Axis on and off here! (0 or 1)
'#####################
'#####################
Const Wheelie = 1                                           'Wheelie = 1, zzaag = 0 (0 or 1)
'#####################
'---------------------------------------------------------------------------
'I2C Part
'---------------------------------------------------------------------------
#if I2c_enable = 1
   $lib "i2c_twi.lbx"
   Config Twi = 400000
   Config Scl = Portc.5
   Config Sda = Portc.4

   Dim Speed_array(4) As Byte
   Dim Speed_left As Word At Speed_array(1) Overlay
   Dim Speed_right As Word At Speed_array(3) Overlay
   Dim Speed_left_out As Integer
   Dim Speed_right_out As Integer
   Dim Speed_sum As Integer
   Dim Speed_single As Single
   Dim Speed_diff As Integer
   Dim Speed_correction As Integer
   Dim Speed_error As Integer
   Declare Sub Gear
#endif
'---------------------------------------------------------------------------
'End I2C Part
'---------------------------------------------------------------------------
'---------------------------------------------------------------------------
'Start X-Axis Part
'---------------------------------------------------------------------------
#if Rocker_enable = 1
   Dim Ad_adxl_r As Long
   Dim Ad_gyro_r As Long
   Dim Total_adxl_gyro_r As Long
   Dim Average_gyro_r As Long
   Dim Tilt_angle_r As Long
   Dim Angle_rate_r As Long
   Dim Buf_r As Long
   Dim Adxl_zero_r As Word
   Dim Gyro_zero_r As Word
   Const Total_looptime_r = 20                              ' Looptime for filters
   Const Total_looptime10_r = 2                             ' Looptime for filters
   Declare Sub Get_rocker_angle
   Dim Test As Long
#endif
'---------------------------------------------------------------------------
'End X-Axis Part
'---------------------------------------------------------------------------
Config Serialin = Buffered , Size = 40 , Bytematch = 13
Config Serialout = Buffered , Size = 40
Echo Off

Pwm_al Alias Ocr1al                                         'Pin 19 PWM to right Motordriver
Pwm_bl Alias Ocr1bl                                         'Pin 18 PWM to left Motordriver
Led3 Alias Portb.4                                          'red LED
Led2 Alias Portb.3                                          'yellow LED
Led1 Alias Portb.2                                          'green LED

Config Watchdog = 32

Ddra = &B00000000
Ddrb = &B00011100
Portb = &B11100011
Ddrc = &B00001000
Portc = &B11111111
Ddrd = &B11110000
Portd = &B00001111

'A complete A/D-conversion uses 13 ADC clock cycles.
'Because of prescaling (divider = 128) 1 cycle takes 104 µs
Config Adc = Single , Prescaler = 128 , Reference = Off
Start Adc

'Timer0 as a 8 bit timer triggers interrupt service routine Ontimer0 every 16,32ms (on overflow).
'To speed up the interrupt intervall Timer0 is preset to 100. Now counting from 100 to 255.
'=> Calcutulation: 155 x 1024 x 62,5 ns = 10 ms
Config Timer0 = Timer , Prescale = 1024
On Timer0 Ontimer0

'Timer1 is used to generate both PWM signals (backward and forward).
'One of them is inverted.
Tccr1a = &B10110001
Tccr1b = 1
Ocr1al = 255

Dim Cw_change_a As Bit
Dim Cw_change_b As Bit
Dim Cw_old_a As Bit
Dim Cw_old_b As Bit
Dim Ad_adxl As Long
Dim Ad_gyro As Long
Dim Ad_batt As Integer
Dim Ad_swi As Integer
Dim Total_adxl_gyro As Long
Dim Average_gyro As Long
Dim Average_batt As Long
Dim Drivea As Integer
Dim Driveb As Integer
Dim Buf1 As Long
Dim Buf As Long
Dim Buf2 As Long
Dim Tilt_angle As Long
Dim Drive_a As Integer
Dim Drive_b As Integer
Dim Drivespeed As Long
Dim Steeringsignal As Long
Dim Anglecorrection As Long
Dim Angle_rate As Long
Dim Balance_moment As Long
Dim Mmode As Byte
Dim Drive_sum As Long
Dim Ad_rocker As Integer
Dim Timeout As Long
Dim Rockersq As Long
Dim Rocker_zero As Integer
Dim Adxl_zero As Long
Dim Gyro_zero As Long
Dim Errno As Byte

'************* Wheelie Hardware *****************
#if Wheelie = 1
Const Ad_kanal_fuss = 7                                     ' AD Kanal Fußschalter
Const Ad_kanal_adxl = 3
Const Ad_kanal_gyro = 5
Const Ad_kanal_batt = 0
Const Ad_kanal_rocker = 6
Const Ad_kanal_rocker_adxl = 2
Const Ad_kanal_rocker_gyro = 4
15v_off Alias Portc.3
Cw_ccw_a Alias Portd.6                                      'Pin 20 Direction right Motordriver
Cw_ccw_b Alias Portd.7                                      'Pin 21 Direction left Motordriver
'***************** Wheelie hardware Ende ************
#else
'***************** Zzaag Hardware ************
Const Ad_kanal_fuss = 4                                     ' AD Kanal Fußschalter
Const Ad_kanal_gyro = 3
Const Ad_kanal_adxl = 1
Const Ad_kanal_batt = 6
Const Ad_kanal_rocker = 5
15v_off Alias Portd.2
Config 15v_off = Output
15v_off = 1
Cw_ccw_a Alias Portd.7                                      'Pin 20 Direction right Motordriver
Cw_ccw_b Alias Portd.6                                      'Pin 21 Direction left Motordriver
#endif
'**************** Zzaag Harware Ende ***************

Const Total_looptime = 150                                  ' Looptime for filters
Const Total_looptime10 = 15                                 ' Looptime for filters

Const _run = 1
Const _standby = 0
Const _down = 3
Const Sw_down = 50                                          'Thershold for foot switch
Const Critical = 100                                        'Time from releasing the foot switch until begin to stop the Wheelie
Const Max_pwm = 180                                         'Maximum motor power
Const Mmax_pwm = -180                                       'Maximum motor power
Const Battdead = 732
Const Battok = 762

Declare Sub Get_tilt_angle
Declare Sub Set_pwm
Declare Sub Algo
Declare Sub Process
Declare Sub Checkbatt
Declare Sub Err_value
Declare Sub Init
Declare Sub Steering

'---------------------------------------------------------------------------
'Telemetry Part
'---------------------------------------------------------------------------
Declare Sub Checkserinp
Declare Sub Parser

Const Mymainchannel = 0
Const Idenditaet = "WOF104"

Dim Serinpstr As String * 63
Dim Serinpbuffer As String * 63
Dim Serinbuffarr(5) As Byte At Serinpbuffer Overlay
Dim Mnemonicbuffer As String * 4
Dim Mnemonicbufferarray(5) As Byte At Mnemonicbuffer Overlay
Dim Cr_received As Byte
Dim Strsize As Byte
Dim Mainchannel As Byte
Dim Subchannel As Word
Dim Wert As Single
Dim Frage As Bit
Dim Zeichenpos As Byte
Dim Zaehler As Byte
Dim Statustext As String * 8
Dim Doppelpunkt As String * 1
Doppelpunkt = ":"
Dim Istgleich As String * 1
Istgleich = "="
'---------------------------------------------------------------------------
'End Telemetry Part
'---------------------------------------------------------------------------
Pwm_al = 255
Pwm_bl = 0
Average_batt = Battok
Mmode = _standby

'LEDshow on start
Set Led1
Waitms 150
Set Led2
Waitms 150
Set Led3
Waitms 250
Reset Led1
Reset Led2
Reset Led3

'---------------------------------------------------------------------------
'Checking foot switch:
'  When the Wheelie is switched on the foot switch should be closed (not pressed down).
'  Otherwise the Wheelie would start. Therefore the foot switch is read 10 times
'  and the mean of this values is compared with a threshold of 100.
'  If this mean is smaller than all is okay. Otherwise an error occurs..
'---------------------------------------------------------------------------

'wait while the sensors drifting at power on.
Enable Interrupts
Gosub Init
Stop Watchdog
Waitms 200
15v_off = 0
Ad_swi = 0
For Buf = 1 To 10
  Ad_swi = Ad_swi + Getadc(ad_kanal_fuss)
  Waitms 1
Next Buf
'calculate middel'
Ad_swi = Ad_swi / 10
Waitms 2
'must be closed
If Ad_swi < 100 Then
  Errno = 4
  Print "2:11=14"
  Goto Err_value
End If

#if Test_enable = 0
   Start Watchdog
   Enable Timer0
   Statustext = Idenditaet
   '---------------------------------------------------------------------------
   'Main Loop:
   '  serial Output of Telemetry Data to PC or Display
   '---------------------------------------------------------------------------
   Do
     If Cr_received = 1 Then                                'wenn was im ser. Buffer ist abholen
         Cr_received = 0
         Input Serinpstr
         'zum Abtrennen eines oder mehrerer LF am Anfang des Strings, das noch vom Vorgänger-String im Buffer steht
         While Asc(serinpstr) = 10
            Strsize = Len(serinpstr) - 1
            Serinpstr = Right(serinpstr , Strsize)
         Wend
         'Hier ist der String empfangen und ein event. LF entfernt
         Call Checkserinp
      End If
     'Das Senden der Telemetriedaten ist noch verbesserungsfähig ;-)
      Waitms 8
      Print "2:1=" ; Str(tilt_angle)
      #if Rocker_enable = 1
         Waitms 8
         Print "2:2=" ; Str(tilt_angle_r)
      #endif
      Waitms 8
      Print "2:3=" ; Str(steeringsignal)
      Waitms 8
      Print "2:4=" ; Str(balance_moment)
      #if I2c_enable = 1
         Waitms 8
         Print "2:5=" ; Str(speed_correction)
         Waitms 8
         Print "2:6=" ; Str(speed_error)
      #endif
      Waitms 8
      Print "2:7=" ; Str(drive_a)
      Waitms 8
      Print "2:8=" ; Str(drive_b)
      #if I2c_enable = 1
         Waitms 8
         Print "2:9=" ; Str(speed_left_out)
         Waitms 8
         Print "2:10=" ; Str(speed_right_out)
      #endif
      If Zaehler > 100 Then
           Zaehler = 0
           Print "2:11=" ; Statustext
      End If
      Waitms 8
      Print "2:12=" ; Str(ad_batt)
   Loop
#else
   Do
      Waitms 5000
      Drive_a = 20
      Gosub Set_pwm
      Waitms 5000
      Drive_b = 20
      Gosub Set_pwm
      Waitms 5000
      Drive_a = 0
      Gosub Set_pwm
      Waitms 5000
      Drive_b = 0
      Gosub Set_pwm
      Waitms 5000
      Drive_a = -20
      Gosub Set_pwm
      Waitms 5000
      Drive_b = -20
      Gosub Set_pwm
      Waitms 5000
      Drive_a = 0
      Gosub Set_pwm
      Waitms 5000
      Drive_b = 0
      Gosub Set_pwm
For Buf = 1 To 1000
Ad_rocker = Getadc(ad_kanal_rocker)
Rockersq = Rocker_zero - Ad_rocker
Steeringsignal = Rockersq
If Steeringsignal > 2 Then
Drive_a = 30
Drive_b = 0
Gosub Set_pwm
End If
If Steeringsignal < -2 Then
Drive_b = 30
Drive_a = 0
Gosub Set_pwm
End If
Waitms 10
Next Buf
Drive_a = 0
Drive_b = 0
Gosub Set_pwm
   Loop
#endif
'---------------------------------------------------------------------------
'Ontimer0:
'  This service routine contains control logic and start every 10ms.
'  Exact time: 1/16 MHz * 1024 * (255-100) = 9,92 ms
'---------------------------------------------------------------------------
Ontimer0:
   Reset Watchdog
   Timer0 = 100
   Gosub Get_tilt_angle
   #if Rocker_enable = 1
      Get_rocker_angle
   #endif
   Gosub Algo
   #if I2c_enable = 1
      Gosub Gear
   #endif
   Gosub Steering
   Gosub Process
   Gosub Set_pwm
   Gosub Checkbatt
   Incr Zaehler
Return

'---------------------------------------------------------------------------
'Get_tilt_angle:
'  Measurement of all analog values (5 values).
'  Using 104 µs per measurement total time is 520 µs.
'  The values are used to build moving averages.
'---------------------------------------------------------------------------
Sub Get_tilt_angle
   Ad_gyro = Getadc(ad_kanal_gyro)
   Ad_gyro = 1024 - Ad_gyro                                 'Inverted value for Ad_gyro
   Ad_adxl = Getadc(ad_kanal_adxl)
   Ad_batt = Getadc(ad_kanal_batt)
   Ad_rocker = Getadc(ad_kanal_rocker)
   Ad_swi = 1024 - Getadc(ad_kanal_fuss)                    'Inverted value for Ad_swi
   Ad_adxl = Ad_adxl * 10
   Ad_gyro = Ad_gyro * 10
   'Subtract   1/250th   of   the   value
   Buf = Total_adxl_gyro / Total_looptime
   Total_adxl_gyro = Total_adxl_gyro - Buf
   Buf = Ad_adxl - Adxl_zero                                'Deviation from horizontal position
   Total_adxl_gyro = Total_adxl_gyro + Buf                  'A new 1/250th value is added

   'Filter for gyro
   Buf1 = Average_gyro / Total_looptime
   Average_gyro = Average_gyro - Buf1                       'Subtract 1/250th of the value
   Average_gyro = Average_gyro + Ad_gyro                    'A new 1/250th value is added
   Buf1 = Average_gyro / Total_looptime10                   'Calculation of angular rate

   Buf = Ad_gyro * 10
   Buf1 = Buf1 - Buf                                        'Deviation from zero
   Buf1 = Buf1 * 30
   Angle_rate = Buf1 / 100                                  '3.5 * deviation rate

   'Calculation of Tilt_angle
   'correct the Tilt_angle with the Average of the gyro.
   'The ADXL-Lowpass Filter terminate all fast Peaks.
   'The Angleratepart rebuilds the correct flanks in Tilt_angle
   Total_adxl_gyro = Total_adxl_gyro + Angle_rate
   Buf = Total_looptime10 * 10
   Tilt_angle = Total_adxl_gyro / Buf
   Angle_rate = Angle_rate / 10
End Sub
'---------------------------------------------------------------------------
'Get_rocker_angle:
'  calculate the tiltangle for the rocker
'  this is done in the same way as Get_tilt_angle
'---------------------------------------------------------------------------
#if Rocker_enable = 1
   Sub Get_rocker_angle
      Ad_adxl_r = Getadc(ad_kanal_rocker_adxl)
      Ad_adxl_r = Ad_adxl_r * 10

      'Subtract   1/250th   of   the   value
      Buf_r = Total_adxl_gyro_r / Total_looptime_r
      Total_adxl_gyro_r = Total_adxl_gyro_r - Buf_r
      Buf_r = Ad_adxl_r - Adxl_zero_r
      Total_adxl_gyro_r = Total_adxl_gyro_r + Buf_r

      Buf = Total_looptime_r * 10
      Buf = Buf / 2.7
      Tilt_angle_r = Total_adxl_gyro_r / Buf
      If Tilt_angle_r > 35 Then Tilt_angle_r = 35
      If Tilt_angle_r < -35 Then Tilt_angle_r = -35
      Tilt_angle_r = Tilt_angle_r / 2
   End Sub
#endif
'---------------------------------------------------------------------------
'Algo:
'  Calculation of motor current out of sensor values.
'---------------------------------------------------------------------------
Sub Algo
   Balance_moment = Tilt_angle + Anglecorrection
   Balance_moment = Angle_rate + Balance_moment
   Balance_moment = Balance_moment * 17
   Drive_sum = Drive_sum + Balance_moment
   If Drive_sum > 55000 Then                                'Limiting Drive_sum
      Drive_sum = 55000
   End If
   If Drive_sum < -55000 Then                               'Limiting Drive_sum
      Drive_sum = -55000
   End If
   Buf = Drive_sum / 155
   Buf1 = Balance_moment / 165
   Drivespeed = Buf + Buf1
End Sub

'---------------------------------------------------------------------------
'Steering:
'  Calculation of steering values.
'---------------------------------------------------------------------------
Sub Steering
   Rockersq = Rocker_zero - Ad_rocker
   #if I2c_enable = 1
      Buf1 = Speed_sum / 70                                 'Steering depends on speed (new and cool)
      Buf1 = Abs(buf1)
      If Buf1 > 15 Then Buf1 = 15
      Buf2 = 30 - Buf1
      Buf2 = Buf2 / 3
      If Rockersq > 35 Then Rockersq = 35
      If Rockersq < -35 Then Rockersq = -35
      Rockersq = Rockersq * Buf2
      Rockersq = Rockersq / 12
      Speed_error = Rockersq - Speed_correction
      If Speed_error > 30 Then Speed_error = 30
      If Speed_error < -30 Then Speed_error = -30
      Rockersq = Rockersq + Speed_error
   #else
      Buf1 = Drive_sum / 20000                              'Steering depends on power (old and ugly)
      Buf1 = Abs(buf1)
      If Buf1 < 1 Then Buf1 = 1
      Rockersq = Rockersq / Buf1
      'Some safety lines, limits steering: Drive_sum 55000 max = +-5
      Buf1 = Drive_sum / 2000
      Buf1 = Abs(buf1)
      If Buf1 > 22 Then Buf1 = 22
      Buf1 = 27 - Buf1
      Buf2 = Buf1 * -1
      If Rockersq > Buf1 Then Rockersq = Buf1
      If Rockersq < Buf2 Then Rockersq = Buf2
   #endif
   Steeringsignal = Rockersq
   Drive_a = Drivespeed - Steeringsignal
   Drive_b = Drivespeed + Steeringsignal
End Sub

'---------------------------------------------------------------------------
'Process:
'  Processing Wheelie states.
'  On standby delete all values, so the motors standing still
'---------------------------------------------------------------------------
Sub Process
   Select Case Mmode
   Case _standby                                            'Foot switch is released:wating...
      Drive_a = 0
      Drive_b = 0
      Drivespeed = 0
      Anglecorrection = 0
      Drive_sum = 0
      Tilt_angle = 0
      Total_adxl_gyro = 0
      Timeout = 0
      Statustext = "Standby"
      If Ad_swi > Sw_down Then                              'If foot switch is pressed: change Mmode to run
         Gosub Init                                         'Initialize sensors
         Mmode = _run
         Statustext = "Drive!"
         Timeout = 0
      End If
   Case _run                                                'Foot switch is pressed: action...
      If Ad_swi < Sw_down Then                              'If foot switch is released change Mmode to down
         Mmode = _down
      End If
   Case _down
         Stop Watchdog
         Timeout = 0
         For Buf1 = 1 To 255
            If Drive_a = 0 And Drive_b = 0 Then
               Exit For
            End If
            If Drive_a > 0 Then Decr Drive_a
            If Drive_a < 0 Then Incr Drive_a
            If Drive_b > 0 Then Decr Drive_b
            If Drive_b < 0 Then Incr Drive_b
            Waitms 25                                       'Loop speed
            Gosub Set_pwm
         Next Buf1
         Mmode = _standby
         Statustext = "Standby"
         Start Watchdog
   Case Else                                                'If an unknown state occurs then slow down
      Mmode = _down
   End Select
End Sub

'---------------------------------------------------------------------------
'Set_pwm:
'  Limiting PWM and set direction flags.
'---------------------------------------------------------------------------
Sub Set_pwm
   'Limiting PWM
   If Drive_a > Max_pwm Then
      Drive_a = Max_pwm
   Elseif Drive_a < Mmax_pwm Then
      Drive_a = Mmax_pwm
   End If
   If Drive_b > Max_pwm Then
      Drive_b = Max_pwm
   Elseif Drive_b < Mmax_pwm Then
      Drive_b = Mmax_pwm
   End If
   Cw_old_a = Cw_ccw_a
   Cw_old_b = Cw_ccw_b
   'Set direction flag
   If Drive_a < 0 Then
      Cw_ccw_a = 1
   Else
      Cw_ccw_a = 0
   End If
   If Drive_b < 0 Then
      Cw_ccw_b = 1
   Else
      Cw_ccw_b = 0
   End If
   If Cw_old_a <> Cw_ccw_a Then
      Set Cw_change_a
   Else
      Reset Cw_change_a
   End If
   If Cw_old_b <> Cw_ccw_b Then
      Set Cw_change_b
   Else
      Reset Cw_change_b
   End If
   Drivea = Abs(drive_a)
   Driveb = Abs(drive_b)

   'Inverse signal with 180° phaseshift to PWMb
   Pwm_al = 255 - Drivea
   Pwm_bl = Driveb
End Sub

'---------------------------------------------------------------------------
'CheckBatt:
'  Battery state (voltage) is signaled with the 3 LEDs.
'---------------------------------------------------------------------------
Sub Checkbatt
   'Moving average of battery voltage
  Buf1 = Average_batt / 300
  Average_batt = Average_batt - Buf1
  Average_batt = Average_batt + Ad_batt
  Buf1 = Average_batt / 300
   'Set the right LED
   If Buf1 > Battok Then
      Set Led1
      Reset Led2
      Reset Led3
   End If
   If Buf1 < Battok Then
      Set Led2
      Reset Led3
      Reset Led1
   End If
   If Buf1 < Battdead Then
      Reset Led2
      Reset Led1
      Set Led3
   End If
End Sub

'---------------------------------------------------------------------------
'Init:
'  Calibration: Calculation of steering mid and  platform horizontal position.
'---------------------------------------------------------------------------
Sub Init
   'Calculate mid position of steering (Steering_zero)
   Stop Watchdog
   Ad_rocker = 0
   For Buf = 1 To 10                                        'Mean out of 10 values
      Ad_rocker = Ad_rocker + Getadc(ad_kanal_rocker)
      Waitms 1
   Next Buf
   Rocker_zero = Ad_rocker / 10
   'Calculate gyro horizontal position of platform
   Ad_gyro = 0
   For Buf = 1 To Total_looptime
      Buf1 = Getadc(ad_kanal_gyro)
      Buf1 = 1024 - Buf1
      Buf1 = Buf1 * 10
      Ad_gyro = Ad_gyro + Buf1
      Waitms 2
   Next Buf
   Gyro_zero = Ad_gyro / Total_looptime
   Average_gyro = Ad_gyro                                   'Preset Average_gyro

   'Calculate ADXL horizontal position of platform
   Ad_adxl = 0
   For Buf = 1 To 100                                       'Mean out of 100 values
      Buf1 = Getadc(ad_kanal_adxl)
      Ad_adxl = Ad_adxl + Buf1
      Waitms 2
   Next Buf
   Adxl_zero = Ad_adxl / 10
#if Rocker_enable = 1
      'Calculate gyro horizontal position of platform
   Ad_gyro_r = 0
   For Buf = 1 To Total_looptime_r
      Buf1 = Getadc(ad_kanal_rocker_gyro)
      Buf1 = Buf1 * 10
      Ad_gyro_r = Ad_gyro_r + Buf1
      Waitms 2
   Next Buf
  Average_gyro_r = Ad_gyro_r
   'Calculate ADXL horizontal position of platform
   Buf1 = 0
   For Buf = 1 To 100                                       'Mean out of 100 values
      Ad_adxl_r = Getadc(ad_kanal_rocker_adxl)
      Buf1 = Ad_adxl_r + Buf1
      Waitms 2
   Next Buf
   Adxl_zero_r = Buf1 / 10
#endif
   'Check if values are in tolerable range
   If Adxl_zero < 4000 Then
      Errno = 3
      Goto Err_value
      Print "2:11=10"
   End If
   If Adxl_zero > 6000 Then
      Errno = 3
      Goto Err_value
      Print "2:11=11"
   End If
   If Gyro_zero < 5000 Then
      Errno = 2
      Goto Err_value
      Print "2:11=12"
   End If
   If Gyro_zero > 6500 Then
      Errno = 2
      Goto Err_value
      Print "2:11=13"
   End If
   Start Watchdog
End Sub

'---------------------------------------------------------------------------
'Err_value:
'  On error the program is processing this routine endlessly.
'  Switch the Wheelie off to reset.
'---------------------------------------------------------------------------
Sub Err_value
   Stop Watchdog
   Do
      For Buf = 1 To Errno
         Set Led3
         Waitms 150
         Reset Led3
         Waitms 150
      Next Buf
      Wait 2
   Loop
End Sub
'---------------------------------------------------------------------------
'     Gear
'
'     get values from the Geartoothsensors
'
'---------------------------------------------------------------------------
#if I2c_enable
Sub Gear
   I2creceive &H40 , Speed_array(1) , 0 , 4                 'Gearsensoren über I²C abfragen
   Speed_left = 11810 / Speed_left                          'umrechnen in km/h *10 damit wir eine Nachkommastelle ohne Single haben
   Speed_right = 11810 / Speed_right                        'der Wert müsste deshalb durch 10 geteilt werden
   If Cw_ccw_b = 1 Then                                     '1181 errechnet sich aus 64µs pro Timertakt des Gearprozessors und 2,1cm Wegstrecke pro Zahnrad
      Speed_left_out = Speed_left * -1
   Else
      Speed_left_out = Speed_left
   End If
   If Cw_ccw_a = 1 Then
      Speed_right_out = Speed_right * -1
   Else
      Speed_right_out = Speed_right
   End If
   Buf = Speed_sum / 5                                      'Mittelwert aus dem Durchschnitt der beiden Geschwindigkeiten bilden
   Speed_sum = Speed_sum - Buf
   Buf = Speed_left_out + Speed_right_out
   Speed_sum = Speed_sum + Buf

   Buf = Speed_diff / 5                                     'Mittelwert aus der Differenz der beiden Geschwindigkeiten bilden
   Speed_diff = Speed_diff - Buf
   Buf = Speed_left_out - Speed_right_out
   Speed_diff = Speed_diff + Buf
   Speed_correction = Speed_diff / 17
End Sub
#endif
'---------------------------------------------------------------------------
'     Serial0charmatch
'
'     Im Eingangspuffer wurde ein "13" empfangen
'     Dieser eine Takt Interrupt verzögert theoretisch die Regelung!?!
'---------------------------------------------------------------------------

Serial0charmatch:
    Cr_received = 1
Return

'---------------------------------------------------------------------------
'     Checkserinp
'
'     Hier ist der String abgeholt
'     Der Mainchannel und der : wird geprüft
'---------------------------------------------------------------------------

Sub Checkserinp
   If Asc(serinpstr) = 35 Then                              'Strings mit # gleich weiterleiten
      Print Serinpstr
      Exit Sub
   End If
   If Asc(serinpstr) = 27 Then                              'Strings mit ESC gleich weiterleiten
      Print Serinpstr
      Exit Sub
   End If
   Zeichenpos = Instr(serinpstr , ":")
   If Zeichenpos > 0 Then                                   ' Doppelpunkt Gefunden
      If Instr(serinpstr , "*") > 0 And Instr(serinpstr , "*") < Zeichenpos Then
         Print Serinpstr
         Call Parser
         Exit Sub
      Else                                                  'kein *
         Serinpbuffer = Left(serinpstr , Zeichenpos)
         Mainchannel = Val(serinpbuffer)
         If Mainchannel = Mymainchannel Then
            Call Parser
            Exit Sub
         Else
            Print Serinpstr
            Exit Sub
         End If
      End If
   Else                                                     'kein :
      If Mainchannel = Mymainchannel Then                   'Mainchannel müsste noch von vorigem Durchlauf drinstehen
         Call Parser
         Exit Sub
      Else                                                  'anderes Modul
         Print Serinpstr
         Exit Sub
      End If
   End If
End Sub
'---------------------------------------------------------------------------
'     Parser
'
'     Der Parser zerteilt die empfangene Zeichenkette in Moduladresse,
'     Subchannel (Befehl) und Wert.
'---------------------------------------------------------------------------
Sub Parser
   Dim I As Byte
   Dim A As Byte
   A = 1
   Serinpbuffer = ""
   Mnemonicbuffer = ""
   Wert = 0
   Subchannel = 0                                           'der ":" und alles davor kommt weg
   Zeichenpos = Len(serinpstr) - Instr(serinpstr , Doppelpunkt)
   Serinpbuffer = Right(serinpstr , Zeichenpos)
   Mnemonicbuffer = ""
   Zeichenpos = Instr(serinpbuffer , Istgleich)
   If Zeichenpos = 0 Then Zeichenpos = Len(serinpbuffer)
   For I = 1 To Zeichenpos                                  'Den ganzen String bis Ende oder = nach Zahlen durchsuchen
       If Serinbuffarr(i) >= "0" And Serinbuffarr(i) <= "9" Then
          Mnemonicbufferarray(a) = Serinbuffarr(i)
          Incr A
          Mnemonicbufferarray(a) = 0
       End If
   Next
   Subchannel = Val(mnemonicbuffer)                         'gefundene Zahl mit dem Mnemonic addieren
   If Instr(serinpbuffer , Istgleich) > 0 Then              '= ist vorhanden
      Frage = 0
   Else
      Frage = 1
   End If
   Zeichenpos = Len(serinpbuffer) - Instr(serinpbuffer , Istgleich)
   Serinpbuffer = Right(serinpbuffer , Zeichenpos)          'alles hinter = ist der Wert
   Wert = Val(serinpbuffer)
   'An dieser Stelle ist alles ausgewertet
   'Mainchannel ist die Kartennummer des Befehls
   'Subchannel ist der Befehl
   'Wert ist der Wert der gesetzt werden soll
   'Frage steht auf 1 wenn kein = im Datensatz vorgekommen ist
   Select Case Subchannel
      Case 1                                                'liest den ADXL-320 Wert
         Serinpstr = "#0:ADX=" + Str(ad_adxl) + "!"
         Print Serinpstr
      Case 2                                                'liest den IDG-300 Wert
         Serinpstr = "#0:IDG=" + Str(ad_gyro) + "!"
         Print Serinpstr
      Case 3
         Serinpstr = "#0:IDN=" + Idenditaet
         Print Serinpstr
      Case 6
         Serinpstr = "#0:BAT=" + Str(ad_batt) + "!"
         Print Serinpstr
      Case Else
         Serinpstr = "#0:ERR=[Syntax Error]!"
         Print Serinpstr
   End Select
End Sub