Help needed for serial communication between BS2

Archiver

hi, i using 4 BS2 at each corner of my robot so that each BS2
control a servo motor and a sensor. The servo motor will stop at 5
different angle to let the sensor scan the range of obstacle. Then,
the BS2 will calculate the average for the sensor reading and
transfer to the main BS2, so called 'Brain". From the Brain BS2, it
decided the appropriate action, such as moves to that direction or
avoids the obstacle.

The problem i facing now is that the sequence of activation of
sensor BS2 is not what i want. The sequence i desired is North
servo turns 1 time, East servo turns 1 time, West servo turns 1 time
and South servo turns 1 time. I attached the sensor BS2 program and
Brain BS2 program as below. The output of the Brain BS2 program is
North servo turns 2 times, East servo turns 1 time, then it goes
back to North servo turns 2 times again, East servo turns 1 time,
West servo turns 1 time and South servo turns 1 time. Can anyone
spot where is the problem for my Brain program? Thanks for your
patience to read through my msg.

SENSOR PROGRAM (same for 4 sersor BS2)

DIRS=%1011111111110101

i VAR Word
j VAR Word
sensor VAR Word
distance VAR Word
sum VAR Word
average VAR Word
n VAR Byte
num_read VAR Byte
lo_average VAR average.LOWBYTE

n=0
sum=0
num_read=0
IN1=0 'pin to activate this BS2
IN3=0

again:
IF IN1=1 THEN left_1
IF IN1=0 THEN again

left_1: 'servo motor start to turn
FOR i=230 TO 238
PULSOUT 0, i
PAUSE 16
NEXT
n=0
GOTO scan

left_2:
i=239
FOR i=239 TO 433
PULSOUT 0, i
PAUSE 16
NEXT
n=1
GOTO scan

center:
i=434
FOR i=434 TO 629
PULSOUT 0, i
PAUSE 16
NEXT
n=2
GOTO scan

right_2:
i=630
FOR i=630 TO 824
PULSOUT 0, i
PAUSE 16
NEXT
n=3
GOTO scan

right_1:
i=825
FOR i=825 TO 1021
PULSOUT 0, i
PAUSE 16
NEXT
n=4
GOTO scan

scan: 'sensor scan environment
OUT15=0 'set output 15 to 0
PAUSE 80 'pause for 80 ms

PULSOUT 15,10 'collecting data bit by bit
sensor.BIT7=IN14
PULSOUT 15,10
sensor.BIT6=IN14
PULSOUT 15,10
sensor.BIT5=IN14
PULSOUT 15,10
sensor.BIT4=IN14
PULSOUT 15,10
sensor.BIT3=IN14
PULSOUT 15,10
sensor.BIT2=IN14
PULSOUT 15,10
sensor.BIT1=IN14
PULSOUT 15,10
sensor.BIT0=IN14

OUT15=1 'set out6 to 1
PAUSE 2 'pause for 2 ms

IF sensor = 0 THEN error
distance=(15600/(sensor-49))-5 'calculate distance of obstacle
IF distance>250 THEN ignore
IF distance<=250 THEN add

ignore:
sum=sum+0
n=n+1
num_read=num_read+0
average=sum/num_read
GOTO counter

add:
sum=sum+distance
n=n+1
num_read=num_read+1
average=sum/num_read
GOTO counter

counter:
IF n=1 THEN left_2
IF n=2 THEN center
IF n=3 THEN right_2
IF n=4 THEN right_1
IF n=5 THEN transfer

transfer:
IN3=1
SEROUT 4\3, 16468, [noparse][[/noparse]lo_average] 'transfer data to Brain program
IN3=0
PAUSE 1000

FOR j=1 TO 780 'servo motor turns back to starting point
PULSOUT 0, 1021-j
PAUSE 16
NEXT
PAUSE 1000
GOTO again

error:
END


BRAIN PROGRAM

DIRS=%1111111110000110 'pin 0,3,4,5 and 6 are set as input

sensor_n VAR Word 'average reading at 5 points North
sensor_e VAR Word 'average reading at 5 points East
sensor_w VAR Word 'average reading at 5 points West
sensor_s VAR Word 'average reading at 5 points South
lo_sensor_n VAR sensor_n.LOWBYTE
lo_sensor_e VAR sensor_e.LOWBYTE
lo_sensor_w VAR sensor_w.LOWBYTE
lo_sensor_s VAR sensor_s.LOWBYTE
sen_limit CON 110

north:
OUT2=1
OUT9=1 'activate north sensor
PAUSE 7000
DEBUG "start to receive data-north", CR
SERIN 3\2,16468,[noparse][[/noparse]lo_sensor_n]'accept data from sensor program
PAUSE 7000
DEBUG "Sensor North=", DEC lo_sensor_n, CR
OUT2=0
PAUSE 7000
IF lo_sensor_n=0 THEN error
IF lo_sensor_n>sen_limit THEN forward_north
IF lo_sensor_n<=sen_limit THEN north_obs

forward_north:
DEBUG "Move forward North", CR
OUT9=0 'deactivate north sensor
PAUSE 8000
GOTO east

east:
OUT2=1
OUT11=1 'activate east sensor
PAUSE 7000
DEBUG "start to receive data-east", CR
SERIN 5\2,16468,[noparse][[/noparse]lo_sensor_e] 'accept data from sensor program
PAUSE 7000
DEBUG "Sensor East=", DEC lo_sensor_e, CR
OUT2=0
PAUSE 7000
IF lo_sensor_e=0 THEN error
IF lo_sensor_e>sen_limit THEN side_east
IF lo_sensor_e<=sen_limit THEN east_obs

side_east:
DEBUG "Move side East", CR
OUT11=0 'deactivate east sensor
PAUSE 8000
GOTO west

west:
OUT2=1
OUT13=1 'activate west sensor
PAUSE 7000
DEBUG "start to receive data-west", CR
SERIN 4\2,16468,[noparse][[/noparse]lo_sensor_w] 'accept data from sensor program
PAUSE 7000
DEBUG "Sensor West=", DEC lo_sensor_w, CR
OUT2=0
PAUSE 7000
IF lo_sensor_w=0 THEN error
IF lo_sensor_w>sen_limit THEN side_west
IF lo_sensor_w<=sen_limit THEN west_obs

side_west:
DEBUG "Move side West", CR
OUT13=0 'deactivate west sensor
PAUSE 8000
GOTO south

south:
OUT2=1
OUT15=1 'activate south sensor
PAUSE 7000
DEBUG "start to receive data-south", CR
SERIN 6\2,16468,[noparse][[/noparse]lo_sensor_s] 'accept data from sensor program
PAUSE 7000
DEBUG "Sensor South=", DEC lo_sensor_s, CR
OUT2=0
PAUSE 7000
IF lo_sensor_s=0 THEN error
IF lo_sensor_s>sen_limit THEN back_south
IF lo_sensor_s<=sen_limit THEN south_obs

back_south:
DEBUG "Move back South", CR
OUT15=0 'deactivate south sensor
PAUSE 8000
GOTO north

north_obs:
DEBUG "Obstacle detected at North!", CR
DEBUG "Check East direction", CR
OUT9=0 'deactivate north sensor
PAUSE 8000
GOTO east

east_obs:
DEBUG "Obstacle detected at East!", CR
DEBUG "Check West direction", CR
OUT11=0 'deactivate east sensor
PAUSE 8000
GOTO west

west_obs:
DEBUG "Obstacle detected at West!", CR
DEBUG "Check South direction", CR
OUT13=0 'deactivate west sensor
PAUSE 8000
GOTO south

south_obs:
DEBUG "Obstacle detected in South.", CR
DEBUG "We were trapped.", CR
OUT15=0 'deactivate north sensor
PAUSE 8000
END

error:
DEBUG "The obstacle are far away.",CR
END

Archiver

Wow, it's really hard to debug it the way
you have it.

I like to have a 'MAIN:' loop, which can hold
the 'top' level logic, then 'CALL' or 'GOTO'
subroutines to do the major functionality.
This way, all I have to do is edit 'MAIN:'
to get new functionality.

This looks like:
SensVal VAR WORD
DirCheck VAR WORD ' 0==North, 1==west, 2==east, 3==south

DirCheck = 4
MAIN:
DirCheck = DirCheck + 1
IF DirCheck < 4 THEN ContMain
' ELSE
DirCheck = 0
ContMain:
IF DirCheck = 0 THEN DoNorth
IF DirCheck = 1 THEN DoWest
IF DirCheck = 2 THEN DoEast
IF DirCheck = 3 THEN DoSouth

MainReturn:
GOTO MAIN
END

' Now, each time through the 'main' loop,
' you are guaranteed to do ONE direction once.
' MUCH easier to debug.
' AND you can easily edit 'main' to change how
' your sensors are checked and moved.
'
' ALSO -- I tend to put a 'goto <somewhere>' after
' EVERY chain if 'if ... THEN ...' -- just to make
' sure the 'fall-through' goes somewhere useful.
'


GOSUB Check_East
IF lo_sensor_e=0 THEN error
IF lo_sensor_e>sen_limit THEN side_east
IF lo_sensor_e<=sen_limit THEN east_obs



--- In basicstamps@yahoogroups.com, "syheng2002" <syheng2002@y...>
wrote:
> hi, i using 4 BS2 at each corner of my robot so that each BS2
> control a servo motor and a sensor. The servo motor will stop at 5
> different angle to let the sensor scan the range of obstacle.
Then,
> the BS2 will calculate the average for the sensor reading and
> transfer to the main BS2, so called 'Brain". From the Brain BS2,
it
> decided the appropriate action, such as moves to that direction or
> avoids the obstacle.
>
> The problem i facing now is that the sequence of activation of
> sensor BS2 is not what i want. The sequence i desired is North
> servo turns 1 time, East servo turns 1 time, West servo turns 1
time
> and South servo turns 1 time. I attached the sensor BS2 program
and
> Brain BS2 program as below. The output of the Brain BS2 program is
> North servo turns 2 times, East servo turns 1 time, then it goes
> back to North servo turns 2 times again, East servo turns 1 time,
> West servo turns 1 time and South servo turns 1 time. Can anyone
> spot where is the problem for my Brain program? Thanks for your
> patience to read through my msg.
>
> SENSOR PROGRAM (same for 4 sersor BS2)
>
> DIRS=%1011111111110101
>
> i VAR Word
> j VAR Word
> sensor VAR Word
> distance VAR Word
> sum VAR Word
> average VAR Word
> n VAR Byte
> num_read VAR Byte
> lo_average VAR average.LOWBYTE
>
> n=0
> sum=0
> num_read=0
> IN1=0 'pin to activate this BS2
> IN3=0
>
> again:
> IF IN1=1 THEN left_1
> IF IN1=0 THEN again
>
> left_1: 'servo motor start to turn
> FOR i=230 TO 238
> PULSOUT 0, i
> PAUSE 16
> NEXT
> n=0
> GOTO scan
>
> left_2:
> i=239
> FOR i=239 TO 433
> PULSOUT 0, i
> PAUSE 16
> NEXT
> n=1
> GOTO scan
>
> center:
> i=434
> FOR i=434 TO 629
> PULSOUT 0, i
> PAUSE 16
> NEXT
> n=2
> GOTO scan
>
> right_2:
> i=630
> FOR i=630 TO 824
> PULSOUT 0, i
> PAUSE 16
> NEXT
> n=3
> GOTO scan
>
> right_1:
> i=825
> FOR i=825 TO 1021
> PULSOUT 0, i
> PAUSE 16
> NEXT
> n=4
> GOTO scan
>
> scan: 'sensor scan environment
> OUT15=0 'set output 15 to 0
> PAUSE 80 'pause for 80 ms
>
> PULSOUT 15,10 'collecting data bit by bit
> sensor.BIT7=IN14
> PULSOUT 15,10
> sensor.BIT6=IN14
> PULSOUT 15,10
> sensor.BIT5=IN14
> PULSOUT 15,10
> sensor.BIT4=IN14
> PULSOUT 15,10
> sensor.BIT3=IN14
> PULSOUT 15,10
> sensor.BIT2=IN14
> PULSOUT 15,10
> sensor.BIT1=IN14
> PULSOUT 15,10
> sensor.BIT0=IN14
>
> OUT15=1 'set out6 to 1
> PAUSE 2 'pause for 2 ms
>
> IF sensor = 0 THEN error
> distance=(15600/(sensor-49))-5 'calculate distance of
obstacle
> IF distance>250 THEN ignore
> IF distance<=250 THEN add
>
> ignore:
> sum=sum+0
> n=n+1
> num_read=num_read+0
> average=sum/num_read
> GOTO counter
>
> add:
> sum=sum+distance
> n=n+1
> num_read=num_read+1
> average=sum/num_read
> GOTO counter
>
> counter:
> IF n=1 THEN left_2
> IF n=2 THEN center
> IF n=3 THEN right_2
> IF n=4 THEN right_1
> IF n=5 THEN transfer
>
> transfer:
> IN3=1
> SEROUT 4\3, 16468, [noparse][[/noparse]lo_average] 'transfer data to Brain
program
> IN3=0
> PAUSE 1000
>
> FOR j=1 TO 780 'servo motor turns back to starting
point
> PULSOUT 0, 1021-j
> PAUSE 16
> NEXT
> PAUSE 1000
> GOTO again
>
> error:
> END
>
>
> BRAIN PROGRAM
>
> DIRS=%1111111110000110 'pin 0,3,4,5 and 6 are set as input
>
> sensor_n VAR Word 'average reading at 5 points North
> sensor_e VAR Word 'average reading at 5 points East
> sensor_w VAR Word 'average reading at 5 points West
> sensor_s VAR Word 'average reading at 5 points South
> lo_sensor_n VAR sensor_n.LOWBYTE
> lo_sensor_e VAR sensor_e.LOWBYTE
> lo_sensor_w VAR sensor_w.LOWBYTE
> lo_sensor_s VAR sensor_s.LOWBYTE
> sen_limit CON 110
>
> north:
> OUT2=1
> OUT9=1 'activate north sensor
> PAUSE 7000
> DEBUG "start to receive data-north", CR
> SERIN 3\2,16468,[noparse][[/noparse]lo_sensor_n]'accept data from sensor program
> PAUSE 7000
> DEBUG "Sensor North=", DEC lo_sensor_n, CR
> OUT2=0
> PAUSE 7000
> IF lo_sensor_n=0 THEN error
> IF lo_sensor_n>sen_limit THEN forward_north
> IF lo_sensor_n<=sen_limit THEN north_obs
>
> forward_north:
> DEBUG "Move forward North", CR
> OUT9=0 'deactivate north sensor
> PAUSE 8000
> GOTO east
>
> east:
> OUT2=1
> OUT11=1 'activate east sensor
> PAUSE 7000
> DEBUG "start to receive data-east", CR
> SERIN 5\2,16468,[noparse][[/noparse]lo_sensor_e] 'accept data from sensor
program
> PAUSE 7000
> DEBUG "Sensor East=", DEC lo_sensor_e, CR
> OUT2=0
> PAUSE 7000
> IF lo_sensor_e=0 THEN error
> IF lo_sensor_e>sen_limit THEN side_east
> IF lo_sensor_e<=sen_limit THEN east_obs
>
> side_east:
> DEBUG "Move side East", CR
> OUT11=0 'deactivate east sensor
> PAUSE 8000
> GOTO west
>
> west:
> OUT2=1
> OUT13=1 'activate west sensor
> PAUSE 7000
> DEBUG "start to receive data-west", CR
> SERIN 4\2,16468,[noparse][[/noparse]lo_sensor_w] 'accept data from sensor
program
> PAUSE 7000
> DEBUG "Sensor West=", DEC lo_sensor_w, CR
> OUT2=0
> PAUSE 7000
> IF lo_sensor_w=0 THEN error
> IF lo_sensor_w>sen_limit THEN side_west
> IF lo_sensor_w<=sen_limit THEN west_obs
>
> side_west:
> DEBUG "Move side West", CR
> OUT13=0 'deactivate west sensor
> PAUSE 8000
> GOTO south
>
> south:
> OUT2=1
> OUT15=1 'activate south sensor
> PAUSE 7000
> DEBUG "start to receive data-south", CR
> SERIN 6\2,16468,[noparse][[/noparse]lo_sensor_s] 'accept data from sensor
program
> PAUSE 7000
> DEBUG "Sensor South=", DEC lo_sensor_s, CR
> OUT2=0
> PAUSE 7000
> IF lo_sensor_s=0 THEN error
> IF lo_sensor_s>sen_limit THEN back_south
> IF lo_sensor_s<=sen_limit THEN south_obs
>
> back_south:
> DEBUG "Move back South", CR
> OUT15=0 'deactivate south sensor
> PAUSE 8000
> GOTO north
>
> north_obs:
> DEBUG "Obstacle detected at North!", CR
> DEBUG "Check East direction", CR
> OUT9=0 'deactivate north sensor
> PAUSE 8000
> GOTO east
>
> east_obs:
> DEBUG "Obstacle detected at East!", CR
> DEBUG "Check West direction", CR
> OUT11=0 'deactivate east sensor
> PAUSE 8000
> GOTO west
>
> west_obs:
> DEBUG "Obstacle detected at West!", CR
> DEBUG "Check South direction", CR
> OUT13=0 'deactivate west sensor
> PAUSE 8000
> GOTO south
>
> south_obs:
> DEBUG "Obstacle detected in South.", CR
> DEBUG "We were trapped.", CR
> OUT15=0 'deactivate north sensor
> PAUSE 8000
> END
>
> error:
> DEBUG "The obstacle are far away.",CR
> END