'{$STAMP BS2}
'************************************************
'* 036_Beacon_Command_1.bas *
'************************************************
'
' This is an experiment using the IRCF-L command 036 - Beacon Command.
' The bot is programmed to follow the direction of strongest IR signal.
' The Bot will just simply turn in the direction of the stongest signal
' See website http://www.robotmaker.eu.com for more details and video of experiment
'************************************************
'* GENERAL VARIABLES *
'************************************************
RX VAR Byte(6)
DATA_ARRAY VAR Nib
' Mathematical variable
LOOPCOUNT VAR Byte 'determine how far Growbot will backup and turn
'************************************************
'* CONTSTANTS *
'************************************************
' Servo Speed Control
MStop CON 750 'Motor Stop
Speed100 CON 125 ' Full Speed
Speed075 CON 50 ' 3/4 speed
Speed050 CON 40 ' 1/2 speed
'Servos
R_SERVO CON 14 'Pin for right servo
L_SERVO CON 13 'Pin for left servo
'LEDS
L_LED CON 15 ' Pin for the left LED
R_LED CON 11 ' Pin for the right LED
'************************************************
'* MAIN PROGRAM *
'************************************************
MAIN:
DEBUG "starting", CR
GOSUB COMMAND_037
CHECK:
SERIN 4, 16468,2000,MAIN,[RX(1),RX(2),RX(3)] ' Get 3 Bytes
GOSUB CHECK_IR
DEBUG DEC RX(1), " ", DEC RX(2)," ", DEC RX(3), " ",CR
GOTO CHECK
'************************************************
'* SUB ROUTINES *
'************************************************
COMMAND_037:
SEROUT 2, 16468, [36]
RETURN
TIME_OUT
DEBUG "TIMEOUT",CR
RETURN
CHECK_IR:
' No Signal
IF RX(1) = 0 AND RX(2)=0 AND RX(3) =0 THEN N
'**Bias left or right
IF RX(1)>0 AND RX(2)>0 AND RX(3) =0 THEN NE
IF RX(1)=0 AND RX(2)>0 AND RX(3) >0 THEN NW
'**Weighing by magnitude
IF RX(2) > RX(1) AND RX(2) > RX(3) THEN N
IF RX(1) > RX(2) AND RX(1) > RX(3) THEN E
IF RX(3) > RX(2) AND RX(3) > RX(1) THEN West
'**clean logic
IF RX(1)=0 AND RX(2)>0 AND RX(3)=0 THEN N
IF RX(1)>0 AND RX(2)=0 AND RX(3)=0 THEN E
IF RX(1)=0 AND RX(2)=0 AND RX(3)>0 THEN West
'**Bias left or right
IF RX(1)>=RX(2) AND RX(2)>=RX(3) THEN East_Bias
IF RX(3)>=RX(2) AND RX(2)>=RX(1) THEN West_Bias
'**Right Detection
IF RX(1)>0 AND RX(1)=RX(3) THEN N 'If both Left and right are equal
'**IR from front direction
IF RX(1)>0 AND RX(1)=RX(3) THEN N 'If both Left and right are equal
GOTO JUMP_ME
EAST_BIAS:
IF RX(2)=RX(1) THEN NE
IF RX(2)>RX(1) THEN N
IF RX(1)>RX(2) THEN E
GOTO JUMP_ME
WEST_BIAS:
IF RX(3)=RX(2) THEN NW
IF RX(2)>RX(3) THEN N
IF RX(3)>RX(2) THEN West
JUMP_ME:
RETURN
'take comments out if your need the bot to move forward
N:
'DEBUG "NORTH", CR
'PULSOUT R_SERVO,MSTOP-speed100 ' Pulse servo
'PULSOUT L_SERVO,MSTOP+speed100 ' Pulse servo
'PAUSE 20
RETURN
WEST:
DEBUG "WEST", CR
LOW L_LED 'Turns on LED
HIGH R_LED 'Turns on LED
FOR LOOPCOUNT=1 TO 5
PULSOUT R_SERVO,MSTOP-speed100 ' Pulse servo
PULSOUT L_SERVO,MSTOP-speed100 ' Pulse servo
PAUSE 20
NEXT
RETURN
E:
DEBUG "EAST", CR
LOW R_LED 'Turns on LED
HIGH L_LED 'Turns on LED
FOR LOOPCOUNT=1 TO 5
PULSOUT R_SERVO,MSTOP+speed100 ' Pulse servo
PULSOUT L_SERVO,MSTOP+speed100 ' Pulse servo
PAUSE 20
NEXT
RETURN
NW:
DEBUG "NORTH WEST", CR
LOW L_LED 'Turns on LED
HIGH R_LED 'Turns on LED
FOR LOOPCOUNT=1 TO 2
PULSOUT R_SERVO,MSTOP-speed100 ' Pulse servo
PULSOUT L_SERVO,MSTOP-speed100 ' Pulse servo
PAUSE 20
NEXT
RETURN
NE:
DEBUG "NORTH EAST", CR
LOW R_LED 'Turns on LED
HIGH L_LED 'Turns on LED
FOR LOOPCOUNT=1 TO 2
PULSOUT R_SERVO,MSTOP+speed100 ' Pulse servo
PULSOUT L_SERVO,MSTOP+speed100 ' Pulse servo
PAUSE 20
NEXT
RETURN
GOTO MAIN
|
TM
