Colony

#!/usr/bin/env python

from scipy import linalg, array

from gtk import *

from socket import *

from struct import *

import gobject

import sys, pickle, pygtk, gtk, string, cairo, gobject, math, weakref

from random import random

#mine

from model import model

pi = 3.1415927

#stolen from emily

BOT_IR = [[4.5, 3.5], [7.5, 0], [4.5, -3.5], [-4, -3.5], [-4, 3.5]]

ANGLE_IR = [[.78, .63], [1, 0], [0.78, -.63], [0, -1], [0,1]]

#endmine

class map_model(model):

    def __init__(self):

        self.eqn = []

    def __setattr__(self,name,value):

        self.__dict__[name] = value

        self.changed()

    def add_eqn(self, eqn):

        self.eqn = [eqn] + self.eqn

hostname = 'localhost'

class Screen(gtk.DrawingArea):

    __gsignals__ = { "expose-event" : "override" }

    def __init__ (self, socket):

        gtk.DrawingArea.__init__(self)

        self.xres, self.yres = (800,800)

        self.set_size_request(self.xres, self.yres)

        self.socket = socket

        self.bots = [i for i in xrange(16)]

        self.ir_pts = []

        self.draw = map_model()

        self.draw.bot = [[1,1,pi/2],0] # x y theta

        self.draw.add_dependent(self.redraw)

        self.scale_const = 50

        self.xdisp = 400

        self.ydisp = 400

        self.nticks = 21

        self.units = 10

    def add_scale_button_ref(self, scale_button):

        self.scale_button = scale_button

    def redraw(self, eqn):

        self.ir_scale = self.scale_button.get_value()

        self.clear_area()

        self.draw_bot()

        self.draw_grid()

        self.draw_ir_pts()

    def do_expose_event(self, event):

        self.redraw(None)

    def clear_area(self):

        self.ctx = self.window.cairo_create()

        self.ctx.rectangle(0,0,1000,1000)

        self.ctx.set_source_rgb(1,1,1)

        self.ctx.fill()

    def draw_ir_pts(self):

        self.ctx = self.window.cairo_create()

        self.ctx.new_path()

        self.ctx.set_source_rgb(0,0,1)

        for pt in self.ir_pts:

            x,y = pt

            self.ctx.move_to(x * self.ir_scale, y * self.ir_scale)

            self.ctx.rel_line_to(2,2)

            self.ctx.stroke()

    def draw_bot(self):

        self.ctx = self.window.cairo_create()

        self.ctx.new_path()

        tbot = self.draw.bot

        print tbot

        x, y, theta = tbot[:3]

        y = -y #because we don't want to draw down more units

        #draw blob

        self.ctx.set_source_rgb(0,1,0)

        xscl = (self.xres - self.xdisp) / self.units

        yscl = (self.yres - self.ydisp) / self.units

        cx = x * xscl + self.xdisp

        cy = y * yscl + self.ydisp

        self.ctx.arc(cx, cy, 20, 0, 2*pi)

        self.ctx.fill()

        #draw "arrow"

        self.ctx.set_source_rgb(0,0,1)

        self.ctx.arc(cx,cy, 5, 0, 2*pi)

        self.ctx.fill()

        self.ctx.move_to(cx,cy)

        dx = 20 * math.cos(theta + 0.05)

        dy = -20 * math.sin(theta + 0.05)

        dx = int(dx)

        dy = int(dy)

        self.ctx.rel_line_to(dx, dy)

        self.ctx.stroke()

    def draw_grid(self):

        self.ctx.set_source_rgb(0,0,0)

        self.ctx.new_path()

        #vertical

        self.ctx.move_to(400,0)

        self.ctx.rel_line_to(0,800)

        self.ctx.stroke()

        #vticks

        self.ctx.move_to(400,0)

        for i in range( self.nticks + 1 ):

            self.ctx.rel_line_to(5,0)

            self.ctx.stroke()

            self.ctx.move_to(400, i * 800 / self.nticks)

        #horiz

        self.ctx.move_to(0,400)

        self.ctx.rel_line_to(800,0)

        self.ctx.stroke()

        #hticks

        self.ctx.move_to(0,400)

        for i in range(self.nticks + 1):

            self.ctx.rel_line_to(0,-5)

            self.ctx.stroke()

            self.ctx.move_to(i * 800 / self.nticks, 400)

    def draw_graph(self, equations, n):

        self.ctx = self.window.cairo_create()

        self.ctx.set_source_rgb(0,0,0)

        self.ctx.new_path()

        for equn in equations:

            xmin, xmax = equn[-2],equn[-1]

            eqn = equn[:-2]

            dx = float(xmax - xmin) / n

            x = xmin

            y = 0

            for i in range(len(eqn)):

                y += eqn[-1 * i - 1] * ( x ** i)

            self.ctx.move_to(self.x_disp + x * self.scale_const,400 - y * self.scale_const)

            for diffel in range(1,n+1):

                x = xmin + diffel * dx

                y = 0

                for i in range(len(eqn)):

                    y += eqn[-1 * i - 1] * ( x ** i)

                self.ctx.line_to(self.x_disp + int(x * self.scale_const),400 - int(y * self.scale_const))

                self.ctx.stroke()

                self.ctx.move_to(self.x_disp + int(x * self.scale_const),400 - int(y * self.scale_const))

    def receive_info(self):

        buf = self.socket.recv(100)

        if (buf):

            items = unpack('4c2hf5h',buf)

            src = ord(items[0])

            old = self.draw.bot #the info of bot src at prev iter

            newx = items[4] / 30.0

            newy = items[5] / 30.0

            new = [newx, newy] + [items[i] for i in range(6,12)]     

            for ir_ind in range(5):

                ir_reading = items[ir_ind + 7]

                if ir_reading  != -1:

                    vec = map(lambda y: ir_reading * y, ANGLE_IR[ir_ind])

                    vec = map(lambda x,y: x+y, vec, BOT_IR[ir_ind])

                    R = math.sqrt(vec[0]**2 + vec[1] ** 2)

                    ttheta = math.atan2(vec[1], vec[0])

                    ttheta += items[6]

                    reading_x = (R * math.cos(ttheta)) + newx

                    reading_y = (R * math.sin(ttheta)) + newy

                    self.ir_pts.append([reading_x, reading_y])

            self.draw.bot = new

        return True

def solve(lisvals):

    p0y, p1y, m0y, m1y, x0, x1 = lisvals

    A = array([[x0**3 , x0**2 , x0 , 1],

               [x1**3 , x1**2 , x1 , 1],

               [3 * x0**2 , 2 * x0 , 1 , 0],

               [3 * x1**2 , 2 * x1 , 1 , 0]])

    B = array([p0y, p1y, m0y, m1y])

    tarr = linalg.solve(A,B)

    lis = []

    for i in range(4):

        lis.append(tarr[i])

    return lis + [0,1]

def main():

    window = gtk.Window()

    window.connect("delete-event", gtk.main_quit)

    sock = socket()

    sock.connect((hostname, int(sys.argv[1])))

    mywidg = Screen(sock)

    bigbox = gtk.HBox()

    bigbox.pack_start(mywidg, True, True, 0)

    buttonbox = gtk.VBox()

    bigbox.pack_end(buttonbox, False, False, 0)

    IRScale = gtk.ScaleButton(10, 0, 100, 10)

    buttonbox.pack_start(IRScale, False, False, 0)

    mywidg.add_scale_button_ref(IRScale)

    window.add(bigbox)

    bigbox.show()

    mywidg.show()

    buttonbox.show()

    IRScale.show()

    window.show()

    gobject.idle_add(mywidg.receive_info)

    gtk.main()

if __name__=='__main__':

    if (len(sys.argv) < 2): #or not str.isdecimal(sys.argv[1])):

        print("Usage: %s <portno>" % sys.argv[0])

        sys.exit()

    main()