'***********************************************************************
'* Name : Complete Robot.BAS *
'* Hard Ware : ET-ROBOT-877 *
'* CPU : PIC 16F877-20P *
'* Date : 8/10/2003 *
'* Program By : Watcharin Kaolop *
'* *
'* Hard Ware Setup : *
'* Servo-L <-- RC2 *
'* Servo-R <-- RC1 *
'* SW bumper left front <-- RA4 *
'* SW bumper right front <-- RE0 *
'* SW bumper left back <-- RE1 *
'* SW bumper right back <-- RA5 *
'* *
'* Notice : This demo program is control robot car by tracker sensor *
'* and switchs bumper sensor and display massage to LCD. *
'* *
'***********************************************************************
' INCLUDE "modedefs.bas" ' Include serial modes
DEFINE OSC 10
pulse_1ms CON 250 ' constant value for pulse 1 ms
pulse_2ms CON 500 ' constant value for pulse 2 ms
Motor_L VAR PORTC.2 ' left servo motor
Motor_R VAR PORTC.1 ' right servo motor
SW_Start VAR PORTA.2
SPK VAR PORTD.1 ' speaker
Run_LED VAR PORTD.0
SW_FL VAR PORTA.4 ' bumper switch in left front position
SW_FR VAR PORTE.0 ' bumper switch in right front position
SW_BL VAR PORTE.1 ' bumper switch in left back position
SW_BR VAR PORTA.5 ' bumper switch in right back position
LFT VAR PORTB.0 ' tracker sensor left position
MID VAR PORTB.1 ' tracker sensor middle position
RGT VAR PORTB.2 ' tracker sensor right position
i VAR Byte
'***** Define configuration for LCD ******
DEFINE LCD_DREG PORTD '
DEFINE LCD_DBIT 4 ' PORTD[7:4] is LCD data pin
DEFINE LCD_RSREG PORTD '
DEFINE LCD_RSBIT 3 ' RD3 is RS pin
DEFINE LCD_EREG PORTD
DEFINE LCD_EBIT 2 ' RD2 is E pin
DEFINE LCD_BITS 4 ' LCD 4 bit mode
'***** Define input and output *****
Output Motor_R ' RC1 is output
Output Motor_L ' RC2 is output
Low Motor_R
Low Motor_L
Input SW_Start
Input SW_FL
Input SW_FR
Input SW_BL
Input SW_BR
TRISB = $FF
TRISD = 0
ADCON1 = 7
Gosub Beep2
LCDOut $FE,1," ET-ROBOT 877",$FE,$C0," (CPU PIC16F877)"
Goto wait_st
stop_press: pause 50
if SW_Start = 0 Then stop_press
Gosub Beep2
wait_st: High Run_LED
IF SW_Start = 1 Then wait_st ' Wait start button (RA2)
wait_1: Pause 50
if SW_Start = 0 Then wait_1
Low Run_LED
'**************************************
' main program *
'**************************************
main:
IF (LFT = 0) AND (MID = 0)AND (RGT = 0) Then Gosub forward
if SW_Start = 0 Then stop_press
IF (LFT = 0) AND (MID = 0)AND (RGT = 1) Then Gosub turn_right ' turn right speed mode
if SW_Start = 0 Then stop_press
IF (LFT = 0) AND (MID = 1)AND (RGT = 0) Then Gosub forward
if SW_Start = 0 Then stop_press
IF (LFT = 0) AND (MID = 1)AND (RGT = 1) Then Gosub turn_norm_R ' turn right normal mode
if SW_Start = 0 Then stop_press
IF (LFT = 1) AND (MID = 0)AND (RGT = 0) Then Gosub turn_left ' turn left speed mode
if SW_Start = 0 Then stop_press
IF (LFT = 1) AND (MID = 0)AND (RGT = 1) Then Gosub forward
if SW_Start = 0 Then stop_press
IF (LFT = 1) AND (MID = 1)AND (RGT = 0) Then Gosub turn_norm_L ' turn left normal mode
if SW_Start = 0 Then stop_press
IF (LFT = 1) AND (MID = 1)AND (RGT = 1) Then Gosub turn_right
if SW_Start = 0 Then stop_press
Goto main
'******************************************
' Walking Format control *
'******************************************
forward: IF SW_FL = 0 Then Gosub evade_left
IF SW_FR = 0 Then Gosub evade_right
PulsOut Motor_L,pulse_2ms ' Generate pulse 2 ms to left servo motor
Low Motor_L ' off
PulsOut Motor_R,pulse_1ms ' Generate pulse 1 ms to right servo motor
Low Motor_R ' off
Pause 18 ' 18 ms
Return
'---------------------------------------------
evade_left: Gosub Beep1
for i = 0 to 30 ' Beeping
GoSub backward ' move to backward
IF SW_BL = 0 Then turn_R45
IF SW_BR = 0 Then turn_L45
Next i
turn_R45: for i = 0 to 30
GoSub turn_norm_R ' turn right 45 degree
Next i
Return
'---------------------------------------------
evade_right: Gosub Beep1
for i = 0 to 30
GoSub backward ' move to backward
IF SW_BL = 0 Then turn_R45
IF SW_BR = 0 Then turn_L45
Next i
turn_L45: for i = 0 to 30
GoSub turn_norm_L ' turn left 45 degree
Next i
Return
'---------------------------------------------
backward: PulsOut Motor_L,pulse_1ms ' Generate pulse 1 ms to left servo motor
Low Motor_L ' off
PulsOut Motor_R,pulse_2ms ' Generate pulse 2 ms to right servo motor
Low Motor_R ' off
Pause 18 ' 18ms
Return
'---------------------------------------------
turn_right: PulsOut Motor_R,pulse_2ms ' Generate pulse 2 ms to right servo motor
Low Motor_R ' off
PulsOut Motor_L,pulse_2ms ' Generate pulse 2 ms to left servo motor
Low Motor_L ' off
Pause 18 ' 18ms
if SW_Start = 0 Then Return
IF SW_FR = 0 Then Return
IF MID = 0 Then turn_right
Return
'---------------------------------------------
turn_left: PulsOut Motor_R,pulse_1ms ' Generate pulse 1 ms to right servo motor
Low Motor_R ' off
PulsOut Motor_L,pulse_1ms ' Generate pulse 1 ms to left servo motor
Low Motor_L ' off
Pause 18 ' 18ms
if SW_Start = 0 Then Return
IF SW_FL = 0 Then Return
IF MID = 0 Then turn_left
Return
'---------------------------------------------
turn_norm_R: Low Motor_R ' off
PulsOut Motor_L,pulse_2ms ' Generate pulse 2 ms to left servo motor
Low Motor_L ' off
Pause 18 ' 18ms
Return
'---------------------------------------------
turn_norm_L: Low Motor_L ' off
PulsOut Motor_R,pulse_1ms ' Generate pulse 1 ms to right servo motor
Low Motor_R ' off
Pause 18 ' 18ms
Return
'---------------------------------------------
Beep1: FreqOut SPK,100,2000 ' Beep
Return
Beep2: FreqOut SPK,100,2000 ' Beep
Pause 100 ' delay
FreqOut SPK,50,2000 ' Beep
Return
