X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fdisplay.py;h=5700edce1f51dd7cc066c3bd7a6d8be868ca41f9;hp=eb63f034e0b768279eb1aa1f3039dad6991c98e3;hb=b56f8565015ec12af0e8df461f8dd52d07f59dd7;hpb=b022f257a5ee568737e1a684a83d0154397fffcb diff --git a/projects/microb2010/mainboard/display.py b/projects/microb2010/mainboard/display.py index eb63f03..5700edc 100644 --- a/projects/microb2010/mainboard/display.py +++ b/projects/microb2010/mainboard/display.py @@ -9,6 +9,10 @@ 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) @@ -23,6 +27,8 @@ 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)) +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)] @@ -41,6 +47,10 @@ 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), @@ -56,8 +66,10 @@ sq2 = square(500) robot_x = 0. robot_y = 0. robot_a = 0. -robot_lspickle = 0 -robot_rspickle = 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 @@ -227,8 +239,15 @@ def toggle_obj_disp(): pos=(x-AREA_X/2,y-AREA_Y/2,CORN_HEIGHT/2)) area_objects.append(c) elif waypoints[i][j] == TYPE_BALL: - c = sphere(radius=50, color=(1., 0.,0.), - pos=(x-AREA_X/2,y-AREA_Y/2,50)) + 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.), @@ -245,33 +264,61 @@ def toggle_obj_disp(): 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(): global robot, last_pos, robot_trail, robot_trail_list global save_pos, robot_x, robot_y, robot_a - axis = (math.cos(robot_a*math.pi/180), - math.sin(robot_a*math.pi/180), + 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.pos = (robot_x - AREA_X/2, robot_y - AREA_Y/2, ROBOT_HEIGHT/2) robot.axis = axis - robot.size = (250, 320, ROBOT_HEIGHT) + robot.size = (ROBOT_LENGTH, ROBOT_WIDTH, ROBOT_HEIGHT) - lspickle.pos = (robot_x - AREA_X/2 + (robot_lspickle*60) * math.cos((robot_a+90)*math.pi/180), - robot_y - AREA_Y/2 + (robot_lspickle*60) * math.sin((robot_a+90)*math.pi/180), + 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, 320, 5) + lspickle.size = (20, ROBOT_WIDTH, 5) + if robot_lspickle_autoharvest: + lspickle.color = (1, 0, 0) + else: + lspickle.color = (0.4, 0.4, 0.4) - rspickle.pos = (robot_x - AREA_X/2 + (robot_rspickle*60) * math.cos((robot_a-90)*math.pi/180), - robot_y - AREA_Y/2 + (robot_rspickle*60) * math.sin((robot_a-90)*math.pi/180), + 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, 320, 5) + rspickle.size = (20, ROBOT_WIDTH, 5) + if robot_rspickle_autoharvest: + rspickle.color = (1, 0, 0) + else: + rspickle.color = (0.4, 0.4, 0.4) # save position - save_pos.append((robot.pos.x, robot.pos, robot_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: @@ -325,25 +372,32 @@ while True: if not m: m = re.match("ballboard=%s"%(INT), l) if m: - print int(m.groups()[0]) + print "ballboard: %d"%(int(m.groups()[0])) # parse cobboard if not m: - m = re.match("cobboard=%s"%(INT), l) + m = re.match("cobboard=%s,%s"%(INT,INT), l) if m: - mode = int(m.groups()[0]) - if (mode & 0x01) == 0: - robot_lspickle = 0 - elif (mode & 0x02) == 0: - robot_lspickle = 1 + #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: + robot_lspickle_deployed = ((flags & 1) * 2) + robot_lspickle_autoharvest = ((flags & 2) != 0) else: - robot_lspickle = 2 - if (mode & 0x04) == 0: - robot_rspickle = 0 - elif (mode & 0x08) == 0: - robot_rspickle = 1 + 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: - robot_rspickle = 2 + fw.write("opp %d %d"%(int(1500 - oppx), int(1050 - oppy))) + except: + print "not connected" if scene.kb.keys == 0: continue @@ -372,6 +426,10 @@ while True: save() elif k == "h": toggle_obj_disp() + elif k == "i": + toggle_color() + else: + print k # EOF if l == "":