' mood_pill.bas by SYNVOX (neni@synvox.ch)
' Version 1.1
' Product: Mood-P.I.L.L.; (Mood Programmable Interactive LED Light).
' Two independent RGB-LEDs controlled by compass heading, tilt,
' or different Programs using HW-PWM (by Color-Hue in 1530 steps).
' Works on ATMEGA168, ATMEGA88.
' Uses Honeywell HMC6532 digital compass module and STmicroelectronics
' LIS2L02AS4 analogue 2D-accelerometer.
' -----FUSES:--------------------------
' Frequency: 8MHz internal RC (remember to disable CKDIV8 fuse bit).
' Disable Reset and set Brown Out Detection to 4.3V.
' -------------------------------------
' Information on Calibration (first time setup):
' 1: To enter Calibration press config_button shortly
' 2: The LED-cross will show you in which direction to point the device to ground
' 3: Press config_button shortly
' 4: When all LEDs are lit, level the device
' 5: Press config_button shortly
' 6: Steps 2 - 6 will cycle infinitely (to exit, power down the device)
' 7: While in calibration mode press config_button long (> 1 sek) to enter compass calibration
' 8: The LEDs in the cross will light up sequentially counterclockwise (10 sek for 1 rotation)
' 9: Turn the device levelled clockwise so that the next LED in the cross lights up approx. at the same spot
'10: After 6 rotations the device will light up all four LEDs in the cross
'11: Power down the device
' -------------------------------------
' Copyright 2007 by SYNVOX (neni@synvox.ch)

' $sim

$regfile = "m168def.dat"
$crystal = 8000000
$hwstack = 64
$swstack = 64
$framesize = 64

$lib "i2c_twi.lbx"                                          ' we do not use software emulated I2C but the TWI

Waitms 1000                                                 ' give Sytem and specifically Compass some time to stabilize

' The highest Moodlight-Sequence-Program number (Mode 2, Programs begin with 0)
Const Maxprog = 7

' The highest Light-Mode (Modes begin with 0)
Const Maxmode = 2

' Compass factor is multiplied with Heading value and then divided by 1000 to give corresponding Hue value
Const Compass_factor = 425

' Pointing to north should give blue color (Heading_offset)
Const Heading_offset = 1500

' maximum tilt value where Compass Heading value is still valid
Const Max_tilt = 15

' Tilt values for Start/Stop function
Const Stop_set = 40
Const Stop_reset = 100

' Delay Constant
Const Delay_time = 1

' I2C-Slave-Address of the HMC6352 Digital Compass Module and Configuration values
Const Compass_address = &H42
Dim Compass_w_array(3) As Byte
Compass_w_array(1) = &H47                                   ' "G"-Command: write to RAM
Compass_w_array(2) = &H74                                   ' RAM Address H74: Operational Mode
Compass_w_array(3) = &B01110000                             ' 20Hz, Set/Reset=yes, Standby mode

' Better human readable definition for the OCR Registers
Red_pwm1 Alias Ocr0a                                        'PWM-Output: Pin 12
Red_pwm2 Alias Ocr0b                                        'PWM-Output: Pin 11
Green_pwm1 Alias Ocr1al                                     'PWM-Output: Pin 15
Green_pwm2 Alias Ocr1bl                                     'PWM-Output: Pin 16
Blue_pwm1 Alias Ocr2a                                       'PWM-Output: Pin 17
Blue_pwm2 Alias Ocr2b                                       'PWM-Output: Pin 05

'The same for input port pins (buttons and jumpers)
Enter_config_and_store Alias Pinc.6                         'enter Configuration and store config values
Mode_button Alias Pinb.6                                    'Cycling through the Modes
Prog_button Alias Pind.7                                    'Cycling through the sequence programs in Mode 2
Speed_button Alias Pinb.0                                   'sets speed in Mode 2 with accelerometer x_value
Store_button Alias Pinb.7                                   'Stores values to EEPROM
Pwm_mode_jumper Alias Pind.0                                'Jumper 1 (J1) sets PWM-Mode for CA or CC LEDs

'The same for LED status outputs
Led_x_plus Alias Portb.5
Led_x_minus Alias Portc.2
Led_y_plus Alias Portc.3
Led_y_minus Alias Portb.4

' Set debounce delay for buttons to 30ms
Config Debounce = 30

' Program variables
Dim P_mode As Byte
Dim Step1 As Word , Step2 As Word
Dim Speed1 As Byte , Speed2 As Byte , Count1 As Byte , Count2 As Byte
Dim Prog1 As Byte , Prog2 As Byte
Dim Progend1 As Byte , Progend2 As Byte
Dim Stop1 As Bit , Stop2 As Bit
Dim Stop_byte As Byte At Stop1 Overlay
Dim Hue(2) As Word
Dim St_state1 As Byte , St_state2 As Byte
Dim User_conf_stat As Byte

' EEPROM Variables for storing program status
Dim E_dummy As Eram Byte                                    'skip location 0 in EEPROM
Dim E_mode As Eram Byte
Dim E_prog1 As Eram Byte , E_prog2 As Eram Byte
Dim E_speed1 As Eram Byte , E_speed2 As Eram Byte
Dim E_stop_byte As Eram Byte

' EEPROM Variables for storing calibration values
Dim E_x_mid As Eram Word , E_x_top As Eram Word , E_x_bottom As Eram Word
Dim E_y_mid As Eram Word , E_y_top As Eram Word , E_y_bottom As Eram Word

' Temporary variables (for calculations and copying)
Dim Hue_val As Word , Temp_b As Byte , Temp_w As Word
Dim Red As Byte , Green As Byte , Blue As Byte
Dim Temp_long As Long

' Predefined system variable for Rnd (Random) function (seeding)
Dim __rseed As Word

' Compass value and accelerometer value read variables
Dim Compass_r_array(2) As Byte
Dim Compass_value As Word At Compass_r_array(1) Overlay
Dim X_tilt As Word , Y_tilt As Word

' Calibration variables (derived and calculated from EEPROM values)
Dim X_mid As Word , X_top As Word , X_bottom As Word , X_diff As Word
Dim Y_mid As Word , Y_top As Word , Y_bottom As Word , Y_diff As Word
Dim Hue_x_factor As Word , Hue_y_factor As Word , Speed_x_factor As Word

' Program Functions and Procedures
Declare Function Prog_call(byval Curr_step As Word , Byval Prog As Byte , Byval Index As Byte) As Byte
Declare Sub Color_set(byval Index As Byte)
Declare Sub Eeprom_read_and_init
Declare Sub Eeprom_write
Declare Sub Read_tilt
Declare Sub Read_compass
Declare Sub Check_mode

' Input/Output Ports initialization
' Inputs: P=Pullup, T=Tristate(HighZ); Outputs: 0=LOW, 1=HIGH
' Port B initialization
' Func7=In Func6=In Func5=Out Func4=Out Func3=Out Func2=Out Func1=Out Func0=In
' State7=P State6=P State5=0 State4=0 State3=0 State2=0 State1=0 State0=P
Portb = &HC1
Ddrb = &H3E

' Port C initialization
' Func6=In Func5=In Func4=In Func3=Out Func2=Out Func1=In Func0=In
' State6=P State5=P State4=P State3=0 State2=0 State1=T State0=T
Portc = &HF0
Ddrc = &H0C

' Port D initialization
' Func7=In Func6=Out Func5=Out Func4=In Func3=Out Func2=In Func1=In Func0=In
' State7=P State6=0 State5=0 State4=P State3=0 State2=P State1=P State0=P
Portd = &H97
Ddrd = &H68

Config Scl = Portc.5                                        ' we need to provide the SCL pin name
Config Sda = Portc.4                                        ' we need to provide the SDA pin name

I2cinit                                                     ' we need to set the pins in the proper state

Config Twi = 100000                                         ' wanted clock frequency
'will set TWBR and TWSR
'Twbr = 32                                                   'bit rate register
'Twsr = 0                                                    ' pre scaler bits

Config Adc = Single , Prescaler = Auto , Reference = Off    ' set ADC to proper configuration

Didr0 = &H03                                                ' disable digital input buffer for ADC0 and ADC1

' Selects PWM-Mode
If Pwm_mode_jumper = 1 Then
   Temp_b = &HA1                                            'non-inverted mode
Else
   Temp_b = &HF1                                            'inverted mode
End If

' Timer/Counter 0 initialization
' Clock source: System Clock
' Clock value: 125 kHz -> PWM-Freq ca. 245 Hz
' Mode: Phase correct PWM top=FFh
' outputs non inverted or inverted depending on Jumper 1 setting
Tccr0a = Temp_b
Tccr0b = &H03
Tcnt0 = &H00
Ocr0a = &H00
Ocr0b = &H00

' Timer/Counter 1 initialization
' Clock source: System Clock
' Clock value: 125 kHz -> PWM-Freq ca. 245 Hz
' Mode: Phase correct PWM top=00FFh
' outputs non inverted or inverted depending on Jumper 1 setting
Tccr1a = Temp_b
Tccr1b = &H03
Tcnt1h = &H00
Tcnt1l = &H00
Icr1h = &H00
Icr1l = &H00
Ocr1ah = &H00
Ocr1al = &H00
Ocr1bh = &H00
Ocr1bl = &H00

' Timer/Counter 2 initialization
' Clock source: System Clock
' Clock value: 125 kHz -> PWM-Freq ca. 245 Hz
' Mode: Phase correct PWM top=FFh
' outputs non inverted or inverted depending on Jumper 1 setting
Assr = &H00
Tccr2a = Temp_b
Tccr2b = &H04
Tcnt2 = &H00
Ocr2a = &H00
Ocr2b = &H00

Waitms 50

I2csend Compass_address , Compass_w_array(1) , 3            ' set Compass Configuration

Waitms 100

' read Calibration values from EEPROM and calculate factors
X_mid = E_x_mid : X_top = E_x_top : X_bottom = E_x_bottom
Y_mid = E_y_mid : Y_top = E_y_top : Y_bottom = E_y_bottom
X_diff = X_top - X_bottom
Y_diff = Y_top - Y_bottom
Temp_long = 1530000 / X_diff
Hue_x_factor = Temp_long
Temp_long = 1530000 / Y_diff
Hue_y_factor = Temp_long
Temp_long = 255000 / X_diff
Speed_x_factor = Temp_long


' reading Program variables from EEPROM and testing for valability
' Initialising the other Program variables
Call Eeprom_read_and_init

' Initialising seed value for Rnd
__rseed = Tcnt0

Call Check_mode

Do

   If P_mode = 0 Then
      Gosub Mode_0
   Elseif P_mode = 1 Then
      Gosub Mode_1
   Elseif P_mode = 2 Then
      Gosub Mode_2
   End If

   Debounce Mode_button , 0 , Set_mode , Sub                ' Mode Button
   Debounce Store_button , 0 , Store_values , Sub           ' Store Button
   If Enter_config_and_store = 0 Then Exit Do               ' Enter Configuration Mode

Loop

Waitms 100
Bitwait Enter_config_and_store , Set
Goto Conf_values

Mode_0:
   Call Read_tilt
   Call Read_compass
   Waitms Delay_time
   Temp_w = X_mid + Max_tilt
   If X_tilt > Temp_w Then
      Led_x_minus = 1
   Else
      Led_x_minus = 0
   End If
   Temp_w = X_mid - Max_tilt
   If X_tilt < Temp_w Then
      Led_x_plus = 1
   Else
      Led_x_plus = 0
   End If
   Temp_w = Y_mid + Max_tilt
   If Y_tilt > Temp_w Then
      Led_y_minus = 1
   Else
      Led_y_minus = 0
   End If
   Temp_w = Y_mid - Max_tilt
   If Y_tilt < Temp_w Then
      Led_y_plus = 1
   Else
      Led_y_plus = 0
   End If
   Temp_w = Compass_value + Heading_offset
   If Temp_w > 3599 Then Temp_w = Temp_w - 3600
   Temp_long = Compass_factor * Temp_w
   Temp_long = Temp_long / 1000
   Hue(2) = Temp_long
   Hue(1) = Hue(2) + 765
   If Hue(1) > 1529 Then Hue(1) = Hue(1) - 1530
   Call Color_set(1)
   Call Color_set(2)
Return

Mode_1:
   Call Read_tilt
   Waitms Delay_time
   If X_tilt <= X_bottom Then
      Temp_w = 0
   Else
      Temp_w = X_tilt - X_bottom
   End If
   Temp_long = Hue_x_factor * Temp_w
   Temp_long = Temp_long / 1000
   Hue(2) = Temp_long
   If Hue(2) > 1530 Then Hue(2) = 1530
   If Y_tilt <= Y_bottom Then
      Temp_w = 0
   Else
      Temp_w = Y_tilt - Y_bottom
   End If
   Temp_long = Hue_y_factor * Temp_w
   Temp_long = Temp_long / 1000
   Hue(1) = Temp_long
   If Hue(1) > 1530 Then Hue(1) = 1530
   Call Color_set(1)
   Call Color_set(2)
Return

Mode_2:
   If Stop1 = 0 And Count1 >= Speed1 Then
      Progend1 = Prog_call(step1 , Prog1 , 1)
      Incr Step1
      If Progend1 = 1 Then Step1 = 0
      Count1 = 1
   End If
   If Stop2 = 0 And Count2 >= Speed2 Then
      Progend2 = Prog_call(step2 , Prog2 , 2)
      Incr Step2
      If Progend2 = 1 Then Step2 = 0
      Count2 = 1
   End If
   If Stop1 = 0 Then Incr Count1
   If Count1 = 0 Then Count1 = 1
   If Stop2 = 0 Then Incr Count2
   If Count2 = 0 Then Count2 = 1

   Call Read_tilt

   ' tilt controlled start/stop function
   If St_state1 = 0 Then
      Temp_w = Y_top - Stop_reset
      If Y_tilt < Temp_w Then St_state1 = 1
   Else
      Temp_w = Y_top - Stop_set
      If Y_tilt > Temp_w Then Gosub Set_stop1
   End If
   If St_state2 = 0 Then
      Temp_w = Y_bottom + Stop_reset
      If Y_tilt > Temp_w Then St_state2 = 1
   Else
      Temp_w = Y_bottom + Stop_set
      If Y_tilt < Temp_w Then Gosub Set_stop2
   End If

   If Stop1 = 1 Then
      Led_y_minus = 1
   Else
      Led_y_minus = 0
   End If
   If Stop2 = 1 Then
      Led_y_plus = 1
   Else
      Led_y_plus = 0
   End If

   Debounce Prog_button , 0 , Set_program , Sub             'Program button
   Debounce Speed_button , 0 , Set_speed , Sub              'Speed button

   Waitus 1000
Return

Set_mode:
   __rseed = Tcnt0                                          'new Seed for Rnd
   Incr P_mode
   Led_x_plus = 0
   Led_x_minus = 0
   Led_y_plus = 0
   Led_y_minus = 0
   Call Check_mode
Return

Store_values:
   __rseed = Tcnt0                                          'new Seed for Rnd
   Call Eeprom_write
Return

Set_program:
   __rseed = Tcnt0                                          'new Seed for Rnd
   If Stop1 = 0 Then
      Step1 = 0
      Count1 = 1
      Incr Prog1
      If Prog1 > Maxprog Then Prog1 = 0
   End If
   If Stop2 = 0 Then
      Step2 = 0
      Count2 = 1
      Incr Prog2
      If Prog2 > Maxprog Then Prog2 = 0
   End If
Return

Set_speed:
   __rseed = Tcnt0                                          'new Seed for Rnd
   If X_tilt <= X_bottom Then
      Temp_w = 0
   Else
      Temp_w = X_tilt - X_bottom
   End If
   Temp_long = Speed_x_factor * Temp_w
   Temp_long = Temp_long / 1000
   Temp_b = Temp_long
   If Temp_b = 0 Then Temp_b = 1
   If Stop1 = 0 Then
      Count1 = 1
      Speed1 = Temp_b
   End If
   If Stop2 = 0 Then
      Count2 = 1
      Speed2 = Temp_b
   End If
Return

Set_stop1:
   __rseed = Tcnt0                                          'new Seed for Rnd
   Toggle Stop1
   St_state1 = 0
Return

Set_stop2:
   __rseed = Tcnt0                                          'new Seed for Rnd
   Toggle Stop2
   St_state2 = 0
Return

Function Prog_call(byval Curr_step As Word , Byval Prog As Byte , Byval Index As Byte) As Byte
   Hue_val = Hue(index)

   If Prog = 0 Then                                         'Rainbow
      Hue_val = Curr_step
      If Curr_step >= 1529 Then
         Prog_call = 1
      Else
         Prog_call = 0
      End If
   Elseif Prog = 1 Then                                     'Rainbow reversed
      Hue_val = 1529 - Curr_step
      If Curr_step >= 1529 Then
         Prog_call = 1
      Else
         Prog_call = 0
      End If
   Elseif Prog = 2 Then                                     'Rainbow alternating
      If Curr_step < 1530 Then
         Hue_val = Curr_step
      Else
         Hue_val = 3060 - Curr_step
      End If
      If Curr_step >= 3059 Then
         Prog_call = 1
      Else
         Prog_call = 0
      End If
   Elseif Prog = 3 Then                                     'Stepping through the 6 basic colors
      If Curr_step < 200 Then                               ' (red, yellow, green, cyan, blue, magenta)
         Hue_val = 0
      Elseif Curr_step < 400 Then
         Hue_val = 255
      Elseif Curr_step < 600 Then
         Hue_val = 510
      Elseif Curr_step < 800 Then
         Hue_val = 765
      Elseif Curr_step < 1000 Then
         Hue_val = 1020
      Else
         Hue_val = 1275
      End If
      If Curr_step >= 1199 Then
         Prog_call = 1
      Else
         Prog_call = 0
      End If
   Elseif Prog = 4 Then                                     'Red - Green gliding
      Hue_val = Curr_step
      If Hue_val > 510 Then Hue_val = 1020 - Hue_val
      If Curr_step >= 1019 Then
         Prog_call = 1
      Else
         Prog_call = 0
      End If
   Elseif Prog = 5 Then                                     'Green - Blue gliding
      Hue_val = Curr_step + 510
      If Hue_val > 1020 Then Hue_val = 2040 - Hue_val
      If Curr_step >= 1019 Then
         Prog_call = 1
      Else
         Prog_call = 0
      End If
   Elseif Prog = 6 Then                                     'Blue - Red gliding
      Hue_val = Curr_step + 1020
      If Hue_val > 1530 Then Hue_val = 3060 - Hue_val
      If Curr_step >= 1019 Then
         Prog_call = 1
      Else
         Prog_call = 0
      End If
   Elseif Prog = 7 Then                                     'Stepping through random Hue
      If Curr_step = 0 Then
         Hue_val = Rnd(1530)
      End If
      If Curr_step >= 199 Then
         Prog_call = 1
      Else
         Prog_call = 0
      End If
   End If

   Hue(index) = Hue_val
   Call Color_set(index)
End Function

Sub Color_set(byval Index As Byte)
   Hue_val = Hue(index)
   If Hue_val < 255 Then
      Red = 255
      Green = Hue_val
      Blue = 0
   Elseif Hue_val < 510 Then
      Red = 510 - Hue_val
      Green = 255
      Blue = 0
   Elseif Hue_val < 765 Then
      Red = 0
      Green = 255
      Blue = Hue_val - 510
   Elseif Hue_val < 1020 Then
      Red = 0
      Green = 1020 - Hue_val
      Blue = 255
   Elseif Hue_val < 1275 Then
      Red = Hue_val - 1020
      Green = 0
      Blue = 255
   Else
      Red = 255
      Green = 0
      Blue = 1530 - Hue_val
   End If
   If Index = 1 Then
      Red_pwm1 = Red
      Green_pwm1 = Green
      Blue_pwm1 = Blue
   Elseif Index = 2 Then
      Red_pwm2 = Red
      Green_pwm2 = Green
      Blue_pwm2 = Blue
   End If
End Sub

Sub Eeprom_read_and_init
   ' reading Program variables from EEPROM and testing for valability
   P_mode = E_mode
   Prog1 = E_prog1
   Prog2 = E_prog2
   Speed1 = E_speed1
   Speed2 = E_speed2
   Stop_byte = E_stop_byte

   If P_mode > Maxmode Then P_mode = 0

   If Prog1 > Maxprog Then Prog1 = 0
   If Prog2 > Maxprog Then Prog2 = 0

   If Speed1 = 0 Then Speed1 = 1
   If Speed2 = 0 Then Speed2 = 1

   ' Initialising the other Program variables
   Step1 = 0
   Step2 = 0
   Count1 = 1
   Count2 = 1
   St_state1 = 0
   St_state2 = 0
End Sub

Sub Eeprom_write
   ' writing the Program variables to EEPROM and giving visual write-feedback
   E_mode = P_mode
   E_prog1 = Prog1
   E_prog2 = Prog2
   E_speed1 = Speed1
   E_speed2 = Speed2
   E_stop_byte = Stop_byte

   Led_x_plus = 1
   Led_x_minus = 1
   Led_y_plus = 1
   Led_y_minus = 1

   Waitms 1000

   Led_x_plus = 0
   Led_x_minus = 0
   Led_y_plus = 0
   Led_y_minus = 0
End Sub

Sub Read_tilt
   X_tilt = Getadc(0)
   Y_tilt = Getadc(1)
End Sub

Sub Read_compass
   Compass_w_array(1) = &H41                                ' Command "A": Read heading value
   I2csend Compass_address , Compass_w_array(1) , 1         ' send to HMC6352
   Waitms 10
   I2creceive Compass_address , Compass_r_array(1) , 0 , 2
   Swap Compass_r_array(1) , Compass_r_array(2)
End Sub

Sub Check_mode
   If P_mode > Maxmode Then P_mode = 0
   If P_mode = 0 Then
      Led_x_plus = 1
      Led_x_minus = 1
      Led_y_plus = 1
      Led_y_minus = 1
      Waitms 500
      Led_x_plus = 0
      Led_x_minus = 0
      Led_y_plus = 0
      Led_y_minus = 0
      Compass_w_array(1) = &H4F                             ' Command "O": Set/Reset Update bridge offsets
      I2csend Compass_address , Compass_w_array(1) , 1      ' send to HMC6352
      Waitms 10
   Elseif P_mode = 1 Then
      Led_x_plus = 1
      Led_x_minus = 1
      Waitms 500
      Led_x_plus = 0
      Led_x_minus = 0
   Elseif P_mode = 2 Then
      Led_y_plus = 1
      Led_y_minus = 1
      Waitms 500
      Led_y_plus = 0
      Led_y_minus = 0
      Step1 = 0
      Step2 = 0
      Count1 = 1
      Count2 = 1
      St_state1 = 0
      St_state2 = 0
   End If
End Sub

Conf_values:
Waitms 100
Temp_b = 0
Led_y_plus = 1
Led_y_minus = 0
Led_x_plus = 0
Led_x_minus = 0
User_conf_stat = 0

Do

   Call Read_tilt
   Waitms 10
   Debounce Enter_config_and_store , 0 , Store_config , Sub
   If User_conf_stat = 1 Then Exit Do                       ' goto HMC6352 user calibration routine

Loop

Waitms 100                                                  ' here comes the user calibration routine
Compass_w_array(1) = &H43                                   ' Command "C": enter user calibration mode
I2csend Compass_address , Compass_w_array(1) , 1            ' send to HMC6352
Waitms 50
For Temp_b = 1 To 6
   Led_y_plus = 1
   Led_y_minus = 0
   Led_x_plus = 0
   Led_x_minus = 0
   Waitms 2500
   Led_y_plus = 0
   Led_x_minus = 1
   Waitms 2500
   Led_x_minus = 0
   Led_y_minus = 1
   Waitms 2500
   Led_y_minus = 0
   Led_x_plus = 1
   Waitms 2500
Next Temp_b
Compass_w_array(1) = &H45                                   ' Command "E": exit user calibration mode
I2csend Compass_address , Compass_w_array(1) , 1            ' send to HMC6352
Waitms 50
Led_y_plus = 1
Led_y_minus = 1
Led_x_plus = 1
Led_x_minus = 1
Do
   !NOP
Loop

Store_config:
   Waitms 1000
   If Enter_config_and_store = 0 Then
      User_conf_stat = 1
   Elseif Temp_b = 0 Then
      E_y_bottom = Y_tilt
      Led_y_plus = 0
      Led_y_minus = 1
   Elseif Temp_b = 1 Then
      E_y_top = Y_tilt
      Led_y_minus = 0
      Led_x_plus = 1
   Elseif Temp_b = 2 Then
      E_x_bottom = X_tilt
      Led_x_plus = 0
      Led_x_minus = 1
   Elseif Temp_b = 3 Then
      E_x_top = X_tilt
      Led_y_plus = 1
      Led_y_minus = 1
      Led_x_plus = 1
   Else
      E_y_mid = Y_tilt
      E_x_mid = X_tilt
      Led_y_minus = 0
      Led_x_plus = 0
      Led_x_minus = 0
   End If
   Incr Temp_b
   If Temp_b > 4 Then Temp_b = 0
Return