symbol servoAngle b0 symbol direction b1 symbol searchMode b2 symbol l

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
symbol servoAngle = b0;
symbol direction = b1;
symbol searchMode = b2;
symbol lastPosition = b3;
symbol nearbyAngleRight = b4;
symbol nearbyAngleLeft = b5;
symbol nearbyDirection = b6;
symbol tmp = b9;
symbol DIRECTION_RIGHT = 0;
symbol DIRECTION_LEFT = 1;
symbol MODE_NORMAL = 0;
symbol MODE_NEARBY = 1;
symbol MAX_RIGHT = 225;
symbol MAX_LEFT = 75;
symbol MAX_ANGLE_NEARBY = 5;
servoAngle = 220; start at center
searchMode = MODE_NORMAL;
direction = DIRECTION_RIGHT;
lastPosition = 150;
nearbyAngleLeft = 0;
nearbyAngleRight = 0;
symbol pinServo = 4
symbol pinSensor = pinC.3
calibfreq 1
pullup on
output pinServo
;high pinServo
input 3
input 2
;servo pinServo,servoAngle
main:
pulsout pinServo, servoAngle
pause 20
goto scan
; else
; pause 50
; endif
; sertxd("servo angle ", #b0, cr, lf)
; if pinSensor = 0 then
; if searchMode = MODE_NEARBY then
; goto slowlySearchNearby
; else
goto scan
; endif
; else
; lastPosition = servoAngle
; searchMode = MODE_NEARBY
; nearbyAngleRight = 0
; nearbyAngleLeft = 0
; endif
;
; goto main
scan:
if direction = DIRECTION_RIGHT then
inc servoAngle
if servoAngle = MAX_RIGHT then
direction = DIRECTION_LEFT
endif
else
dec servoAngle
if servoAngle = MAX_LEFT then
direction = DIRECTION_RIGHT
endif
endif
goto main
searchNearby:
if nearbyDirection = DIRECTION_LEFT then
moveLeft:
if nearbyAngleLeft = MAX_ANGLE_NEARBY then endNearbySearch
inc nearbyAngleLeft
let servoAngle = lastPosition - nearbyAngleLeft
nearbyDirection = DIRECTION_RIGHT
if servoAngle < MAX_LEFT then
direction = DIRECTION_LEFT;
goto moveRight
endif
else
moveRight:
if nearbyAngleRight = MAX_ANGLE_NEARBY then endNearbySearch
inc nearbyAngleRight
let servoAngle = lastPosition + nearbyAngleRight
if servoAngle > MAX_RIGHT then
direction = DIRECTION_LEFT;
goto moveLeft
endif
nearbyDirection = DIRECTION_LEFT
endif
goto main
endNearbySearch:
; sertxd("nearby search ended", cr, lf)
if servoAngle = MAX_LEFT then
direction = DIRECTION_RIGHT
endif
if servoAngle = MAX_RIGHT then
direction = DIRECTION_LEFT
endif
searchMode = MODE_NORMAL
goto main
slowlySearchNearby:
; sertxd("looking right", cr, lf)
for tmp = 0 TO MAX_ANGLE_NEARBY
servoAngle = lastPosition + tmp
if servoAngle > MAX_RIGHT then slowlyLookLeft
sertxd("servo angle ", #b0, cr, lf)
servopos pinServo,servoAngle
if pinSensor = 1 then main
next
for tmp = 0 TO MAX_ANGLE_NEARBY
servoAngle = lastPosition + tmp
if servoAngle > MAX_RIGHT then slowlyLookLeft
sertxd("servo angle ", #b0, cr, lf)
servopos pinServo,servoAngle
if pinSensor = 1 then main
next
slowlyLookLeft:
; sertxd("looking left", cr, lf)
for tmp = 0 TO MAX_ANGLE_NEARBY
servoAngle = lastPosition - tmp
if servoAngle < MAX_LEFT then endNearbySearch
; sertxd("servo angle ", #b0, cr, lf)
servopos pinServo,servoAngle
if pinSensor = 1 then main
next
searchMode = MODE_NORMAL
goto main