; object avoiding robot
init:
input A.0,A.1,A.1
output B.1,B.2,B.3
high B.1,B.2,B.3 ;leds off
pwmout B.0, 52, 105 ; 38000Hz at 50% @ 8MHz
pwmout pwmdiv16, C.1, 124, 0 ; 1000Hz at 0% @ 8MHz motors
pwmout pwmdiv16, C.2, 124, 0 ; 1000Hz at 0% @ 8MHz off
;lmotordir = B.6
;rmotordir = B.7
;lled = pinB.1
;fled = pinB.2
;rled = pinB.3
;lsens = pinA.0
;fsens = pinA.1
;rsens = pinA.2
main:
low B.2 ; front led on
if pinA.1 = 0 then ; forward is blocked?
pwmduty C.1,0 ; motors off
pwmduty C.2,0
high B.2 ;front led off
low B.3 ; right led on
else goto goforward
endif
if pinA.2 = 1 then ; right is clear
low B.3 ; right led off
high B.6 ; left motor forward
low B.7 ; right motor reverse
pwmduty C.1,249 ; spin clock wise
pwmduty C.2,249 ; 50% speed
low B.2 ; front led on
do until pinA.1 = 1 ; spin clockwise until
loop ; no object
high B.2 ; front led off
pwmduty C.1,0 ; motors
pwmduty C.2,0 ; off
goto goforward
endif
; right is blocked so reverse for 1/2 second then spin half second
low B.1 ; left led on
if pinA.0 = 0 then ; left blocked
high B.1 ; left led off
low B.6 ; left motor reverse
low B.7 ; right motor reverse
pwmduty C.1,249 ; left motor 50%
pwmduty C.2,249 ; right motor 50%
pause 500 ; reverse for 1/2 second
pwmduty C.1,0 ; motors
pwmduty C.2,0 ; off
high B.6; left motor forward
low B.7 ; right motor reverse
pwmduty C.1,249 ; spin clock wise
pwmduty C.2,249 ; 50% speed
pause 500 ; for 1/2 second
end if
goforward:
high B.6 ; motors
high B.7 ; forward
pwmduty C.1,499 ; motors
pwmduty C.2,499 ; full
goto main