#slot 0
#picaxe 40X2
Setfreq M8
#No_Data
#No_Table
DisableBod
pause 100
symbol ModeSw = 11 'Mode switch analogue input
symbol Mode = b0
symbol Counter = B2 'counter
symbol Counter1 = B3
symbol counter2 = b6
symbol LeftREV = b10 'Left motor rev counter variable
symbol RightREV = b11 'Right motor rev counter variable
symbol REVCount = w6
symbol LeftWallDet = 23 'analog input from Interrupt detector
symbol RightWallDet = 13 'analog input from Interrupt detector
symbol Reverse_Range = b16
symbol Randirection = b17
symbol directionselect = b4
symbol RevLatch = b5
symbol deadband = b18
symbol randomhigh = b26 'random number high byte
symbol randomlow = b27
symbol CMPEnter = b14
symbol CMPNline = b15
symbol LeftDistance = b20
symbol RightDistance = b22
symbol CMPHundred = b50
symbol CMPtens = b51
symbol CMPUnits = b52
symbol CMPdp = b53
symbol CMPtenth = b54
symbol CMPSum = B55
symbol ModeSel = w12 'Mode switch variable
symbol Randnum = w13
Symbol range = w14
symbol randelay = w15
symbol templeftPath = w16
symbol tempRange = w17
symbol temprightPath = w18
Symbol leftPath = w19 'Obstruction value on left side
Symbol rightPath = w20 'Obstruction value on Right side
symbol ObjectRight = w21 'Obstruction distance on Right side
Symbol Rrange = w22
symbol ObjectLeft = w23 'Obstruction distance on Left side
symbol edgedata = w24
Symbol trig = A.6 'Head Scanner SRF005
Symbol Rtrig = A.7 'Rear Scanner SRF005
symbol TrigLeft = A.3 'Left Front Scanner SRF005
symbol TrigRight = A.5 'Right Front Scanner SRF005
Symbol SERVOH = b.0
symbol M1FWD = D.7 'Motor 1 Froward output
symbol M1REV = D.6 'Motor 1 Reverse output
Symbol M2FWD = D.5 'Motor 2 Froward output
symbol RGB2 = D.2 'RGB LED Bit 2
Symbol M2REV = D.4 'Motor 2 Reverse output
symbol OLED = C.0 'serial output to OLED display
Symbol M1PWM = C.2 'Motor 1 Speed (PWM) output
Symbol CompassSerialIn = C.1 'Serial input from compass
symbol RRevIn = pinc.5 'motor 1 Speed Detector
symbol LRevIn = pinc.6 'motor 2 Speed Detector
symbol Overload = pinc.7 'motor Overload input
symbol Select1 = pinb.1
Symbol SERVOLeft = 150
Symbol SERVORight = 450
Symbol SERVOCent = 280
symbol cmpbaud = t9600 'SET SERIAL OUTPUT baud rate for GY-26
symbol Baud = N2400 'Set serial output baud rate for OLED
symbol vshortdelay = 150
symbol shortdelay = 450
symbol middelay = 2000
symbol longdelay = 2200
Roam:
' gosub DistanceMessure 'check if forward direction is clear
serout OLED,Baud,(254,128)
serout OLED,Baud,(" Roaming " )
gosub FWDFast 'set bot to forward direction
pause randelay
counter1 = counter1 + 1 'get a random delay value for turn counter then add 1 to count
if Counter1 = 10 then
gosub randomselect
if Randirection =1 then
LeftREV = 7
gosub turnleft
else
LeftREV = 7
gosub turnright
endif '
gosub FWDFast 'turn bot to the left when counter = 10
' gosub DistanceMessure
endif
if Counter1 = 20 then 'turn bot to the right when counter = 20
gosub randomselect
counter1 = 0
if Randirection =1 then
LeftREV = 7 ' reset counter
gosub turnright
else
LeftREV = 7
gosub turnleft
endif
gosub FWDFast
endif
goto Roam
randomselect:
' generate random time delays when in micro control
' w3 contains the random number gerated each time this
' routine is called and randelay is set to value determine by the
' value in Randnum
random Randnum
if randomlow > 200 then
let randelay = 600
let Randirection =1
return
endif
if randomlow > 150 then
let randelay=500
return
endif
if randomlow > 100 then
let randelay=400
let Randirection =0
return
endif
if randomlow > 75 then
let randelay=200
let Randirection =1
return
endif
if randomlow > 50 then
let randelay=100
let Randirection =0
return
endif
let randelay=50
let Randirection =1
return
TurnLeft:
b6 =5
' gosub rgbcolour
Revcount = 0
serout OLED,Baud,(254,192)
serout OLED,Baud,(" Turning Left " )
High M1PWM
high M1FWD
low M1Rev
Low M2FWD
Low M2REV
do ' Count Wheel rotations 7 pulses = approx 90Deg rotation
do : loop until rRevIn = 0
do : loop until rRevIn = 1
RevCount = RevCount + 1
loop until RevCount >= 7
Revcount = 0
return
TurnRight:
b6 =6
' gosub rgbcolour
Revcount = 0
serout OLED,Baud,(254,192)
serout OLED,Baud,(" Turning Right " )
do
High M1PWM
Low M1FWD
low M1Rev
High M2FWD
Low M2REV
do : loop until lRevIn = 0
do : loop until lRevIn = 1
RevCount = RevCount + 1
loop until RevCount >= 7
Revcount = 0
Return
FwdFast:
b6 =1
' gosub rgbcolour
serout OLED,Baud,(254,192)
serout OLED,Baud,(" Forward " )
High M1PWM
high M1FWD
low M1Rev
High M2FWD
Low M2REV
Return
BackFast:
b6 =7
' gosub rgbcolour
serout OLED,Baud,(254,192)
serout OLED,Baud,(" Reversing " )
High M1PWM
high M1REV
low M1FWD
High M2REV
Low M2FWD
for b6 = 1 to 10 'When reverse is called, first check if reverse diredtion is clear. (check 10 times)
pulsout Rtrig,2 ' produces about 20uS pulse (must be minimum of 10uS)
pulsin Rtrig,1,Rrange ' measures the range in 10uS steps
pause 10 ' SRF04 mandatory 10mS recharge period after ranging completes
let Rrange = Rrange * 10 /58/2
if Rrange < 20 then
exit ' if obstical is detected
endif
pause 100
next b6
Return
StopM:
b6 =8
' gosub rgbcolour
serout OLED,Baud,(254,192)
serout OLED,Baud,(" Stopped " )
Low M1PWM
Low M1REV
low M1FWD
Low M2REV
Low M2FWD
Return
TurnAround:
serout OLED,Baud,(254,192)
pause 10
serout OLED,Baud,(" Turning Round " )
do
High M1PWM
Low M1FWD
high M1Rev
High M2FWD
Low M2REV
do : loop until RRevIn =0
do : loop until RRevIn = 1
RevCount = RevCount + 1
loop until RevCount >= 12
Return