-import math, sys, time, os, random
+import math, sys, time, os, random, re
from visual import *
+FLOAT = "([-+]?[0-9]*\.?[0-9]+)"
+INT = "([-+]?[0-9][0-9]*)"
+
AREA_X = 3000.
AREA_Y = 2100.
+ROBOT_HEIGHT=5 # 350
+CORN_HEIGHT=5 # 150
+BALL_CYLINDER=1 # 0
+
+ROBOT_WIDTH=320
+ROBOT_LENGTH=250
+
area = [ (0.0, 0.0, -0.2), (3000.0, 2100.0, 0.2) ]
areasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , area)
area_box = box(size=areasize, color=(0.0, 1.0, 0.0))
# all positions of robot every 5ms
save_pos = []
-robot = box(pos = (0, 0, 150),
- size = (250,320,350),
- color = (1, 0, 0) )
+robot = box(color=(0.4, 0.4, 0.4))
+lspickle = box(color=(0.4, 0.4, 0.4))
+rspickle = box(color=(0.4, 0.4, 0.4))
-last_pos = robot.pos.x, robot.pos.y, robot.pos.z
+opp = box(color=(0.7, 0.2, 0.2))
+
+last_pos = (0.,0.,0.)
hcenter_line = curve()
hcenter_line.pos = [(-AREA_X/2, 0., 0.3), (AREA_X/2, 0., 0.3)]
vcenter_line = curve()
vcenter_line.pos = [(0., -AREA_Y/2, 0.3), (0., AREA_Y/2, 0.3)]
+yellowarea = [ (0.0, 0.0, -0.5), (500.0, 500.0, 0.5) ]
+yellowareasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , yellowarea)
+yellowarea_box = box(pos=(-AREA_X/2+250,-AREA_Y/2+250,0), size=yellowareasize, color=(1.0, 1.0, 0.0))
+
+bluearea = [ (0.0, 0.0, -0.5), (500.0, 500.0, 0.5) ]
+blueareasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , bluearea)
+bluearea_box = box(pos=(AREA_X/2-250,-AREA_Y/2+250,0), size=blueareasize, color=(0.0, 0.0, 1.0))
+
+greyarea = [ (0.0, 0.0, -0.5), (1520.0, 500.0, 0.5) ]
+greyareasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , greyarea)
+greyarea_box = box(pos=(0,-AREA_Y/2+250,0), size=greyareasize, color=(0.3, 0.6, 0.3))
+
+YELLOW = 0
+BLUE = 1
+color = YELLOW
+
def square(sz):
sq = curve()
sq.pos = [(-sz, -sz, 0.3),
sq1 = square(250)
sq2 = square(500)
+robot_x = 0.
+robot_y = 0.
+robot_a = 0.
+robot_lspickle_deployed = 0
+robot_rspickle_deployed = 0
+robot_lspickle_autoharvest = 0
+robot_rspickle_autoharvest = 0
robot_trail = curve()
robot_trail_list = []
max_trail = 500
TYPE_WHITE_CORN=2
TYPE_BLACK_CORN=3
TYPE_OBSTACLE=4
-TYPE_NEIGH=5
+TYPE_BALL=5
+TYPE_NEIGH=6
col = [TYPE_WAYPOINT] * WAYPOINTS_NBY
waypoints = [col[:] for i in range(WAYPOINTS_NBX)]
if c >= 0:
waypoints[i][j] = corn_table[c]
continue
+
+ # balls
+ if (i & 1) == 0 and j > 3 and \
+ (not (i == 0 and j == WAYPOINTS_NBY-1)) and \
+ (not (i == WAYPOINTS_NBX-1 and j == WAYPOINTS_NBY-1)):
+ waypoints[i][j] = TYPE_BALL
+ continue
+ if (i == 0 or i == WAYPOINTS_NBX-1) and j > 2 and j < 7:
+ waypoints[i][j] = TYPE_BALL
+ continue
+
# too close of border
if (i & 1) == 1 and j == WAYPOINTS_NBY -1:
waypoints[i][j] = TYPE_OBSTACLE
def toggle_obj_disp():
global area_objects
+ """
+ if area_objects == []:
+ c = sphere(radius=5, color=(0., 0.,1.),
+ pos=(1238.-AREA_X/2, 1313.-AREA_Y/2, 5))
+ area_objects.append(c)
+ c = sphere(radius=5, color=(0., 0.,1.),
+ pos=(1364.-AREA_X/2, 1097.-AREA_Y/2, 5))
+ area_objects.append(c)
+ c = sphere(radius=5, color=(0., 0.,1.),
+ pos=(1453.-AREA_X/2, 1176.-AREA_Y/2, 5))
+ area_objects.append(c)
+ c = sphere(radius=5, color=(0., 0.,1.),
+ pos=(1109.-AREA_X/2, 1050.-AREA_Y/2, 5))
+ area_objects.append(c)
+"""
if area_objects == []:
i = 0
j = 0
while y < 2100:
print x,y
if waypoints[i][j] == TYPE_WHITE_CORN:
- c = cylinder(axis=(0,0,1), length=150,
+ c = cylinder(axis=(0,0,1), length=CORN_HEIGHT,
radius=25, color=(0.8,0.8,0.8),
- pos=(x-AREA_X/2,y-AREA_Y/2,75))
+ pos=(x-AREA_X/2,y-AREA_Y/2,CORN_HEIGHT/2))
area_objects.append(c)
elif waypoints[i][j] == TYPE_BLACK_CORN:
- c = cylinder(axis=(0,0,1), length=150,
+ c = cylinder(axis=(0,0,1), length=CORN_HEIGHT,
radius=25, color=(0.2,0.2,0.2),
- pos=(x-AREA_X/2,y-AREA_Y/2,75))
+ pos=(x-AREA_X/2,y-AREA_Y/2,CORN_HEIGHT/2))
+ area_objects.append(c)
+ elif waypoints[i][j] == TYPE_BALL:
+ if BALL_CYLINDER == 1:
+ c = cylinder(axis=(0,0,1), radius=50,
+ length=CORN_HEIGHT,
+ color=(1., 0.,0.),
+ pos=(x-AREA_X/2,y-AREA_Y/2,CORN_HEIGHT/2))
+ else:
+ c = sphere(radius=50, color=(1., 0.,0.),
+ pos=(x-AREA_X/2,y-AREA_Y/2,50))
+
+ area_objects.append(c)
+ else:
+ c = sphere(radius=5, color=(0., 0.,1.),
+ pos=(x-AREA_X/2,y-AREA_Y/2,5))
area_objects.append(c)
j += 1
y += STEP_CORN_Y
else:
o.visible = 1
+def toggle_color():
+ global color
+ global BLUE, YELLOW
+ if color == YELLOW:
+ color = BLUE
+ else:
+ color = YELLOW
+
+def set_opp(x, y):
+ opp.size = (300, 300, ROBOT_HEIGHT)
+ opp.pos = (x, y, ROBOT_HEIGHT/2)
-def set_robot(x, y, a):
+def set_robot():
global robot, last_pos, robot_trail, robot_trail_list
- global save_pos
+ global save_pos, robot_x, robot_y, robot_a
- robot.pos = (x - AREA_X/2, y - AREA_Y/2, 150)
- robot.axis = (math.cos(a*math.pi/180),
- math.sin(a*math.pi/180),
- 0)
- robot.size = (250, 320, 350)
+ if color == YELLOW:
+ tmp_x = robot_x - AREA_X/2
+ tmp_y = robot_y - AREA_Y/2
+ tmp_a = robot_a
+ else:
+ tmp_x = -robot_x + AREA_X/2
+ tmp_y = -robot_y + AREA_Y/2
+ tmp_a = robot_a
+
+ robot.pos = (tmp_x, tmp_y, ROBOT_HEIGHT/2)
+ axis = (math.cos(tmp_a*math.pi/180),
+ math.sin(tmp_a*math.pi/180),
+ 0)
+
+ robot.axis = axis
+ robot.size = (ROBOT_LENGTH, ROBOT_WIDTH, ROBOT_HEIGHT)
+
+ lspickle.pos = (tmp_x + (robot_lspickle_deployed*60) * math.cos((tmp_a+90)*math.pi/180),
+ tmp_y + (robot_lspickle_deployed*60) * math.sin((tmp_a+90)*math.pi/180),
+ ROBOT_HEIGHT/2)
+ lspickle.axis = axis
+ lspickle.size = (20, ROBOT_WIDTH, 5)
+ if robot_lspickle_autoharvest:
+ lspickle.color = (0.2, 0.2, 1)
+ else:
+ lspickle.color = (0.4, 0.4, 0.4)
+
+ rspickle.pos = (tmp_x + (robot_rspickle_deployed*60) * math.cos((tmp_a-90)*math.pi/180),
+ tmp_y + (robot_rspickle_deployed*60) * math.sin((tmp_a-90)*math.pi/180),
+ ROBOT_HEIGHT/2)
+ rspickle.axis = axis
+ rspickle.size = (20, ROBOT_WIDTH, 5)
+ if robot_rspickle_autoharvest:
+ rspickle.color = (0.2, 0.2, 1)
+ else:
+ rspickle.color = (0.4, 0.4, 0.4)
# save position
- save_pos.append((robot.pos.x, robot.pos, a))
+ save_pos.append((robot.pos.x, robot.pos.y, tmp_a))
pos = robot.pos.x, robot.pos.y, 0.3
if pos != last_pos:
except:
pass
-init_corn_table(random.randint(0,8), random.randint(0,3))
+#init_corn_table(random.randint(0,8), random.randint(0,3))
+init_corn_table(0, 0)
waypoints = init_waypoints()
toggle_obj_disp()
fr = open("/tmp/.robot_sim2dis", "r")
fw = open("/tmp/.robot_dis2sim", "w", 0)
while True:
+ m = None
l = fr.readline()
- try:
- x,y,a = map(lambda x:int(x), l[:-1].split(" "))
- set_robot(x,y,a)
- except ValueError:
- pass
+
+ # parse position
+ if not m:
+ m = re.match("pos=%s,%s,%s"%(INT,INT,INT), l)
+ if m:
+ robot_x = int(m.groups()[0])
+ robot_y = int(m.groups()[1])
+ robot_a = int(m.groups()[2])
+ set_robot()
+
+ # parse ballboard
+ if not m:
+ m = re.match("ballboard=%s"%(INT), l)
+ if m:
+ print "ballboard: %d"%(int(m.groups()[0]))
+
+ # parse cobboard
+ if not m:
+ m = re.match("cobboard=%s,%s"%(INT,INT), l)
+ if m:
+ print "cobboard: %x,%x"%(int(m.groups()[0]),int(m.groups()[1]))
+ side = int(m.groups()[0])
+ flags = int(m.groups()[1])
+ if (side == 0 and color == YELLOW) or (side == 1 and color == BLUE):
+ robot_lspickle_deployed = ((flags & 1) * 2)
+ robot_lspickle_autoharvest = ((flags & 2) != 0)
+ else:
+ robot_rspickle_deployed = ((flags & 1) * 2)
+ robot_rspickle_autoharvest = ((flags & 2) != 0)
+
+ if scene.mouse.events != 0:
+ oppx, oppy, oppz = scene.mouse.getevent().project(normal=(0,0,1))
+ set_opp(oppx, oppy)
+ try:
+ if color == YELLOW:
+ fw.write("opp %d %d"%(int(oppx + 1500), int(oppy + 1050)))
+ else:
+ fw.write("opp %d %d"%(int(1500 - oppx), int(1050 - oppy)))
+ except:
+ print "not connected"
if scene.kb.keys == 0:
continue
save()
elif k == "h":
toggle_obj_disp()
+ elif k == "i":
+ toggle_color()
+ else:
+ print k
# EOF
if l == "":