'/**************************************/;
'/* Program Demo For ET-ROBOT RD2 V1.0 */;
'/* Controller  : P89C51RD2(Philips)   */;
'/* Run X-TAL   : 18.432 Mhz           */;
'/*             : X2 Mode(6 Cycle Run) */;
'/* Complier    : BASCOM-51(V2.0.11.0) */;
'/* Write By    : Eakachai Makarn      */;
'/* File Update : December-2003        */;
'/* Used PCA Generate PWM Control Servo*/;
'/* Demo Sensor Switch(Input Byte Read)*/;
'/**************************************/;
'
$regfile = "89c51rd.dat"                                      'P89C51RD2(Philips)
$ramstart = 0
$ramsize = 256
$crystal = 36864000                                           '18.432MHz (X2 Mode)

Config Timer0 = Timer , Gate = Internal , Mode = 2            'Timer0 = 8Bit Auto Reload
Load Timer0 , 240                                             'Timer0 Count = 240x325.52ns = 78.125uS
Disable Timer0                                                'Disable Timer0 Interrupt
Start Timer0                                                  'Start Timer0 Trig PCA Counter
Cmod = &B00000100                                             'CPS(1:0) = 10 (Timer0 Overflow Trigger)
Ccapm1 = &B01000010                                           'Set Bit ECOM1,PWM1 (Enable PWM1)
Ccapm2 = &B01000010                                           'Set Bit ECOM2,PWM2 (Enable PWM2)
Ccap1h = 0                                                    'PWM1 Duty Cycle = 20mS
Ccap2h = 0                                                    'PWM2 Duty Cycle = 20mS
Ccon = &B01000000                                             'Set Bit CR (Turn-on PCA Timer)

Declare Sub Robot_forward                                     'Forward Direction
Declare Sub Robot_backward                                    'Backward Direction
Declare Sub Robot_fast_left                                   'Turn Left Fast
Declare Sub Robot_slow_left                                   'Turn Left Slow
Declare Sub Robot_fast_right                                  'Turn Right Fast
Declare Sub Robot_slow_right                                  'Turn Right Slow
Declare Sub Robot_stop                                        'Robot Stop

Declare Sub Back_turn_right                                   'Front Left Service
Declare Sub Back_turn_left                                    'Front Right Service
Declare Sub Back_uturn                                        'Front Left+Right Sevice
Declare Sub Next_turn_right                                   'Back Left Service
Declare Sub Next_turn_left                                    'Back Right Service

Dim Input_new As Byte                                         'Input New Status
Dim Input_old As Byte                                         'Input old Status
Port_input Alias P0                                           'Port Input

Robot_stop                                                    'Stop Robot & Wait Switch-Start
Bitwait P0.2 , Reset                                          'Wait Until SW-Start Press (P0.2="0")
Robot_forward                                                 'Start Robot
Input_old = &B00011011                                        'Default Input Bit 0,1,3,4

Do
  Input_new = Port_input
  Input_new = Input_new And &B00011011                        'Check Input Bit 0,1,3,4 only
  If Input_new <> Input_old Then
    Waitms 30                                                 'Debounce
    Input_new = Port_input
    Input_new = Input_new And &B00011011                      'Check Input Bit 0,1,3,4 only
    If Input_new <> Input_old Then
       Input_old = Input_new
       Select Case Input_new
         Case &B00010010 : Back_uturn                         'Sw (Front-Left)+(Front-Right)
         Case &B00011010 : Back_turn_right                    'Sw Front-Left only
         Case &B00010011 : Back_turn_left                     'Sw Front-Right only
         Case &B00011001 : Next_turn_right                    'Sw Back-Left only
         Case &B00001011 : Next_turn_left                     'Sw Back-Right only
       Case Else Robot_forward                                'Other Case Forward
       End Select
    End If
  End If
Loop


'/*****************/;
'/* ROBOT Forward */;
'/*****************/;
'
Sub Robot_forward                                             'Forward Direction
  Ccap1h = 256 - 26                                           'Pwm1 Duty Cycle Reload(2.0ms) = Forward
  Ccap2h = 256 - 13                                           'Pwm2 Duty Cycle Reload(1.0ms) = Forward
End Sub

'/******************/;
'/* ROBOT Backward */;
'/******************/;
'
Sub Robot_backward                                            'Backward Direction
  Ccap1h = 256 - 13                                           'Pwm1 Duty Cycle Reload(1.0ms)= Backward
  Ccap2h = 256 - 26                                           'Pwm2 Duty Cycle Reload(2.0ms)= Backward
End Sub

'/*************************/;
'/* ROBOT Turn Left(Fast) */;
'/*************************/;
'
Sub Robot_fast_left                                           'Turn Left Fast
  Ccap1h = 256 - 13                                           'Pwm1 Duty Cycle Reload(1.0ms)= Backward
  Ccap2h = 256 - 13                                           'Pwm2 Duty Cycle Reload(1.0ms)= Forward
End Sub

'/*************************/;
'/* ROBOT Turn Left(Slow) */;
'/*************************/;
'
Sub Robot_slow_left                                           'Turn Left Slow
  Ccap1h = 0                                                  'Pwm1 Duty Cycle Reload(20ms="1") = Stop
  Ccap2h = 256 - 13                                           'Pwm2 Duty Cycle Reload(1.0ms)= Forward
End Sub

'/**************************/;
'/* ROBOT Turn Right(Fast) */;
'/**************************/;
'
Sub Robot_fast_right                                          'Turn Right Fast
  Ccap1h = 256 - 26                                           'Pwm1 Duty Cycle Reload(2.0ms)= Forward
  Ccap2h = 256 - 26                                           'Pwm2 Duty Cycle Reload(2.0ms)= Backward
End Sub

'/**************************/;
'/* ROBOT Turn Right(Slow) */;
'/**************************/;
'
Sub Robot_slow_right                                          'Turn Right Fast
  Ccap1h = 256 - 26                                           'Pwm1 Duty Cycle Reload(2.0ms)= Forward
  Ccap2h = 0                                                  'Pwm2 Duty Cycle Reload(20ms="1")= Stop
End Sub

'/**************/;
'/* ROBOT Stop */;
'/**************/;
'
Sub Robot_stop                                                'Robot Stop
  Ccap1h = 0                                                  'Pwm1 Duty Cycle Reload(20ms="1") = Stop
  Ccap2h = 0                                                  'Pwm2 Duty Cycle Reload(20ms="1") = Stop
End Sub

Sub Back_turn_right                                           'Switch Front-Right Service
 Robot_backward
 Wait 1
 Robot_fast_right
 Wait 1
 Robot_forward
End Sub

Sub Back_turn_left                                            'Switch Front-Left Service
 Robot_backward
 Wait 1
 Robot_fast_left
 Wait 1
 Robot_forward
End Sub

Sub Back_uturn                                                'Switch (Front-Left)+(Front-Right) Service
 Robot_backward
 Wait 1
 Robot_fast_left
 Wait 2
 Robot_forward
End Sub

Sub Next_turn_right                                           'Switch Back-Right Service
 Robot_forward
 Wait 1
 Robot_fast_right
 Wait 1
 Robot_forward
End Sub

Sub Next_turn_left                                            'Switch Back-Left Service
 Robot_forward
 Wait 1
 Robot_fast_left
 Wait 1
 Robot_forward
End Sub

End

