'---------------------------------------------------------------------------
'  _____ _     _   _              _ _ _ _           _ _
' |   __| |___| |_| |_ ___ ___   | | | | |_ ___ ___| |_|___
' |   __| | -_| '_|  _| . |  _|  | | | |   | -_| -_| | | -_|
' |_____|_|___|_,_|_| |___|_|    |_____|_|_|___|___|_|_|___|
'
' (c)2009 Elektor International Media B.V.
'---------------------------------------------------------------------------
'Version: Speed100 - Peripheral Measurements for Elektor
'---------------------------------------------------------------------------
'Purpose:
'  Measurement of Speed, Current and direction of rotation for the Elektor-Wheelie
'  Byte 1 and 2: Speed left
'  Byte 3 and 4: Speed right
'  Byte 5 and 6: Current left
'  Byte 7 and 8: Current right
'  Byte 9 Bit 6: left Direction
'  Byte 9 Bit 7: No Direction Flag left
'  Byte 9 Bit 3: right Direction
'  Byte 9 Bit 4: No Direction Flag right
'---------------------------------------------------------------------------
'Description:
'  Until now only speed is calculated out of gear tooth sensor sensors
'---------------------------------------------------------------------------'
'Changes Log:
'  Speed090: 11.12.2009
'     First code by Günter Gerold
'  Speed091: 11.12.2009
'     Handling of Timer1 overflow by Thomas Scherer
'  Speed092: 12.12.2009
'     Fallback to Wordmax if no Interrupts after 2 Timer1 Overflows
'  Speed093: 13.12.2009
'     Fallback to 10000 if no Interrupts after 228,48 ms (slower than 0,333 km/h)
'     Timer0 and Timer2
'  Speed094: 11.07.2010
'     Measure the currents of the Sensors.
'     for a short time of the ADC we dont wait for the result
'     if the ADC is ready we store the value in the interrupt and switch the channel.
'  Speed100: 25.12.2010
'     detection of the drivingdirection with the ATS657 45µs-Pulse = forward, 90µs Pulse = backward
'---------------------------------------------------------------------------
'ToDo:
'  nothing
'---------------------------------------------------------------------------
'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
'---------------------------------------------------------------------------
$regfile = "m88def.dat"
$crystal = 16000000
$baud = 57600

'timeout when waiting for pulses
Config Timer0 = Timer , Prescale = 1024
On Timer0 Int_timer0
Start Timer0
'measure the distance between two pulses (speed)
Config Timer1 = Timer , Prescale = 1024       'Time base: timer clock = 15625Hz = 64µs/pulse
Start Timer1
'measure the width of the pulse (direction)
Config Timer2 = Timer , Prescale = 64       '4µs per Tick
Start Timer2
'Sensor inputs generate interrupts at rising and falling flank
Config Int0 = Change          'Interrupt for the left tooth sensor
Config Int1 = Change          'Interrupt for the right tooth sensor
On Int0 Int_0
On Int1 Int_1
'measure Currentsensors
On Adc On_adc                 'ADC Interrupt
Admux = &B01000000
Adcsra = &B11001111
Dim Cur_ch As Bit

'all Data stored in an Array, so its easy to send with I²C
Dim Data_array(10) As Byte
Dim Speed_left As Word At Data_array(1) Overlay
Dim Speed_right As Word At Data_array(3) Overlay
Dim Curr_left As Word At Data_array(5) Overlay
Dim Curr_right As Word At Data_array(7) Overlay
Dim Direction_byte As Byte At Data_array(9) Overlay

Dim Count_left_old As Word    'detect Timer1 overflow
Dim Count_right_old As Word   'detect Timer1 overflow
Dim Timer_left As Word        'Value of Timer1 left motor
Dim Timer_right As Word       'Value of Timer1 right motor
Dim Timer0_overflow_l As Byte
Dim Timer0_overflow_r As Byte
Dim Timer2_up_l As Byte
Dim Timer2_down_l As Byte
Dim Timer2_up_r As Byte
Dim Timer2_down_r As Byte
Dim Left_direction As Byte
Dim Right_direction As Byte


Dim Cnt As Byte
Dim Twi_control As Byte
Dim Twi_status As Byte

Const Wordmax = 65535
Const Bytemax = 256

Enable Interrupts
Enable Int0
Enable Int1
Enable Timer0
Enable Timer2
Twsr = 0
Twdr = &HFF
Twar = &H40                   'Our I²C Adress
Twcr = &B01000100             'Hardware TWI Slave Transmitter
Speed_left = 10000
Speed_right = 10000
Do
   If Timer0_overflow_l > 14 Then       '229ms and no Toothinterrupt (255 x 1024 x 14 x 62,5ns = 229ms), we don't wait longer and set a value.
        Timer0_overflow_l = 0
        Speed_left = 10000    'Set 1/Speed to MAX = 0,12 km/h
        Set Direction_byte.7  'no Direction
   End If
   If Timer0_overflow_r > 14 Then
        Timer0_overflow_r = 0
        Speed_right = 10000
        Set Direction_byte.4  'no Direction
   End If

   Select Case Right_direction
      Case 5 To 17            'CW detected
         Set Direction_byte.3
         Reset Direction_byte.4
      Case 18 To 28           'CCW detected
         Reset Direction_byte.3
         Reset Direction_byte.4
      Case 38 To 53           'no Direction detected
         Set Direction_byte.4
   End Select
   Select Case Left_direction
      Case 5 To 17            'CW detected
         Set Direction_byte.6
         Reset Direction_byte.7
      Case 18 To 28           'CCW detected
         Reset Direction_byte.6
         Reset Direction_byte.7
      Case 38 To 53           'no Direction detected
         Set Direction_byte.7
   End Select
   Twi_control = Twcr And &H80
   If Twi_control = &H80 Then 'Interruptflag of the Hardware TWI is set
      Twi_status = Twsr And &HF8
      Select Case Twi_status
         Case &HA8:           'Send the first Byte
            Twdr = Data_array(1)
            Twcr = &B11000100
            Cnt = 2
         Case &HB8:           'Send additional Bytes
            Twdr = Data_array(cnt)
            Incr Cnt
            Twcr = &B11000100
         Case Else :          'Ignore other Commands
            Twcr = &B11000100
      End Select
      If Cnt = 10 Then Adcsra.6 = 1       'If all Data is send via I²C then start the ADC for new current values
   End If
Loop

Int_0:                        'A changing Flank was detected from the left Sensor
   If Pind.2 = 1 Then         'the rising Flank
      Timer0_overflow_l = 0
      Timer_left = Timer1     'Timer1 measures the distance between the pulses
      Timer2_up_l = Timer2    'Timer2 measures the width of the pulse
      If Timer_left >= Count_left_old Then
         Speed_left = Timer_left - Count_left_old
      Else
         Speed_left = Wordmax - Count_left_old       'Overflow of Timer1
         Speed_left = Speed_left + Timer_left
      End If
      Count_left_old = Timer_left       'store old Value
      If Speed_left > 10000 Then Speed_left = 10000
   Else                       'the falling Flank
      Timer2_down_l = Timer2  'calculate the width of the pulse
      If Timer2_down_l > Timer2_up_l Then
         Left_direction = Timer2_down_l - Timer2_up_l
      Else
         Left_direction = Bytemax - Timer2_up_l
         Left_direction = Left_direction + Timer2_down_l
      End If
   End If
Return

Int_1:                        'A changing Flank was detected from the left Sensor
   If Pind.3 = 1 Then         'the rising Flank
      Timer0_overflow_r = 0
      Timer_right = Timer1
      Timer2_up_r = Timer2
      If Timer_right >= Count_right_old Then
         Speed_right = Timer_right - Count_right_old
      Else                    'Overflow of Timer1
         Speed_right = Wordmax - Count_right_old
         Speed_right = Speed_right + Timer_right
      End If
      Count_right_old = Timer_right
      If Speed_right > 10000 Then Speed_right = 10000
   Else                       'the falling Flank
      Timer2_down_r = Timer2  'calculate the width of the pulse
      If Timer2_down_r > Timer2_up_r Then
         Right_direction = Timer2_down_r - Timer2_up_r
      Else
         Right_direction = Bytemax - Timer2_up_r
         Right_direction = Right_direction + Timer2_down_r
      End If
   End If
Return

Int_timer0:                   'count Timer0 Overflows for Timeout of the Wheels
   Incr Timer0_overflow_l
   Incr Timer0_overflow_r
Return

On_adc:                       'ADC is ready and have a new value
   If Cur_ch = 0 Then         'value from the left Sensor
        Curr_left = Adc
        Toggle Cur_ch         'switch to the right sensor
        Admux.0 = Cur_ch
        Adcsra.6 = 1          'start ADC to get the value from the right sensor
   Else
        Curr_right = Adc
        Toggle Cur_ch
        Admux.0 = Cur_ch      'switch to the left sensor, but dont start the ADC. This was made later in the mainloop.
   End If
Return