| 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
|
from pyrobot.geometry import * # import distance function
from pyrobot.brain.behaviors import State, FSMBrain
class edge (State):
def onActivate(self): # method called when activated or gotoed
self.startX = self.robot.x
self.startY = self.robot.y
def step(self):
x = self.robot.x
y = self.robot.y
dist = distance( self.startX, self.startY, x, y)
print "EDGE: actual = (%f, %f) start = (%f, %f); dist = %f" \
% (x, y, self.startX, self.startY, dist)
if dist > 1.0:
self.goto('turn')
else:
self.robot.move(.3, 0)
class turn (State):
def onActivate(self):
self.th = self.robot.th
def step(self):
th = self.robot.th
print "TURN: actual = %f start = %f" % (th, self.th)
if angleAdd(th, - self.th) > 90:
self.goto('edge')
else:
self.robot.move(0, .2)
def INIT(engine):
brain = FSMBrain(engine=engine)
# add a few states:
brain.add(edge(1)) # 1 makes it active
brain.add(turn())
return brain |
