'*******************************************************************
' Program name mini_car
' Start Date 18th.Jan.2010
' Author Yoshio KATO
' Description : Used MCU ATtiny2313
'             : System Clock 8MHz Internal OSC
'             : PD5=LED Right  PD0=LED Left
'             : PD1=Right Sencor  PD2= Left Sencor PD3=Start Switch  PD4=Center Switch
'             : PB0,PB1=Right Motor Control PB2,PB3=Left Motor Control
'*******************************************************************
$regfile = "attiny2313.dat"
$crystal = 8000000                                          'internal clock 8MHz
Dim Wk As Byte
Dim I As Byte
Dim Count As Byte
Const Led_r = 5                                             'Right LED
Const Led_l = 0                                             'Left LEd
Const Sw_r = 1                                              'Right sensor switch
Const Sw_l = 2                                              'Left sensor switch
Const Sw_st = 3                                             'Start switch
Const Sw_c = 4                                              'center secor switch
Const Mr_in1 = 0                                            'Right Motor control TA7291P IN1
Const Mr_in2 = 1                                            'Right Motor control TA7291P IN2
Const Ml_in1 = 2                                            'Left Motor control TA7291P IN1
Const Ml_in2 = 3                                            'Left Motor control TA7291P IN2
Main:
   Stop Ac                                                  ' Analog comparater power off
   Config Portb = Output
   Config Portd = &HE1                                      'Output
   Portd = &B00011110                                       'PORTD input pin pullup
   Do
      Gosub Stop_all
      Do                                                    'Check start switch
         Debounce Pind.3 , 0 , Go_forward
         Set Portd.led_r                                    'led right on
         Set Portd.led_l                                    'led left on
         Waitms 200                                         'wait 0.5sec
         Reset Portd.led_r                                  'led right off
         Reset Portd.led_l                                  'led left off
         Waitms 200
      Loop
Go_forward:
      Wait 1
      Do
         Gosub Forward
         Count = Count + 1
         If Pind.sw_c = 0 Then                              'center swich on
            Wk = Count Mod 2
            If Wk = 1 Then                                  'if wk is odd, turn to left
               Gosub Turn_to_left
              Else
               Gosub Turn_to_right
            End If
         End If

         If Pind.sw_r = 0 Then                              'right sencor on ?
            If Pind.sw_l = 0 Then                           'left sencor on ?
               Wk = Count Mod 2
               If Wk = 1 Then                               'odd
                  Gosub Turn_to_left
                 Else
                  Gosub Turn_to_right
               End If
              Else                                          'right sensor only on
               Gosub Turn_to_left                           ' turn to left
            End If
         End If
         If Pind.sw_l = 0 Then                              'left sensor on?
            If Pind.sw_r = 0 Then                           'right sensor on? both sencor on
               Wk = Count Mod 2
               If Wk = 1 Then                               'odd
                  Gosub Turn_to_left
                Else
                  Gosub Turn_to_right
               End If
             Else
               Gosub Turn_to_right
            End If
         End If
         If Pind.sw_st = 0 Then
            Exit Do
         End If
      Loop
   Loop
End



'*******************************************************************
' Forward
'*******************************************************************
Forward:
   Reset Portb.mr_in1
   Set Portb.mr_in2
   Reset Portb.ml_in1
   Set Portb.ml_in2
Return

'*******************************************************************
' Back
'*******************************************************************
Back:
   Set Portb.mr_in1
   Reset Portb.mr_in2
   Set Portb.ml_in1
   Reset Portb.ml_in2
   For I = 1 To 4                                           'both direction indicator blink
      Set Portd.led_r
      Set Portd.led_l
      Waitms 125
      Reset Portd.led_r
      Reset Portd.led_l
      Waitms 125
   Next I
Return

'*******************************************************************
'Turn_to Right
'*******************************************************************
Turn_to_right:
   Gosub Back                                               'Turn to right
   Set Portb.mr_in1
   Reset Portb.mr_in2
   Reset Portb.ml_in1
   Set Portb.ml_in2
   For I = 1 To 4                                           'right direction indicator blink
      Set Portd.led_r
      Waitms 125
      Reset Portd.led_r
      Waitms 125
   Next I
Return

'*******************************************************************
' Turn to left
'*******************************************************************
Turn_to_left:
   Gosub Back                                               'Turn to left
   Reset Portb.mr_in1
   Set Portb.mr_in2
   Set Portb.ml_in1
   Reset Portb.ml_in2
   For I = 1 To 4                                           'left direction indicator blink
      Set Portd.led_l
      Waitms 125
      Reset Portd.led_l
      Waitms 125
   Next I
Return

'*******************************************************************
' Stop all wheel
'*******************************************************************
Stop_all:                                                   'Stop                                '
   Reset Portb.mr_in1
   Reset Portb.mr_in2
   Reset Portb.ml_in1
   Reset Portb.ml_in2
Return