Revision 918
python stuff
| trunk/code/projects/mapping/python/map.py | ||
|---|---|---|
| 1 |
#!/usr/bin/env python |
|
| 2 |
|
|
| 3 |
from scipy import linalg, array |
|
| 4 |
from gtk import * |
|
| 5 |
from socket import * |
|
| 6 |
from struct import * |
|
| 7 |
import gobject |
|
| 8 |
import sys, pickle, pygtk, gtk, string, cairo, gobject, math, weakref |
|
| 9 |
from random import random |
|
| 10 |
|
|
| 11 |
#mine |
|
| 12 |
from model import model |
|
| 13 |
|
|
| 14 |
pi = 3.1415927 |
|
| 15 |
|
|
| 16 |
#endmine |
|
| 17 |
|
|
| 18 |
class map_model(model): |
|
| 19 |
def __init__(self): |
|
| 20 |
self.eqn = [] |
|
| 21 |
|
|
| 22 |
def __setattr__(self,name,value): |
|
| 23 |
self.__dict__[name] = value |
|
| 24 |
self.changed() |
|
| 25 |
|
|
| 26 |
def add_eqn(self, eqn): |
|
| 27 |
self.eqn = [eqn] + self.eqn |
|
| 28 |
|
|
| 29 |
hostname = 'localhost' |
|
| 30 |
|
|
| 31 |
class Screen(gtk.DrawingArea): |
|
| 32 |
__gsignals__ = { "expose-event" : "override" }
|
|
| 33 |
|
|
| 34 |
def __init__ (self, socket): |
|
| 35 |
gtk.DrawingArea.__init__(self) |
|
| 36 |
|
|
| 37 |
self.xres, self.yres = (800,800) |
|
| 38 |
|
|
| 39 |
self.set_size_request(self.xres, self.yres) |
|
| 40 |
|
|
| 41 |
|
|
| 42 |
self.socket = socket |
|
| 43 |
self.bots = [] |
|
| 44 |
|
|
| 45 |
self.draw = map_model() |
|
| 46 |
self.draw.bot = [1,1,pi/2] # x y theta |
|
| 47 |
self.draw.add_dependent(self.redraw) |
|
| 48 |
|
|
| 49 |
self.scale_const = 50 |
|
| 50 |
self.xdisp = 400 |
|
| 51 |
self.ydisp = 400 |
|
| 52 |
self.nticks = 21 |
|
| 53 |
self.units = 10 |
|
| 54 |
|
|
| 55 |
def redraw(self, eqn): |
|
| 56 |
self.clear_area() |
|
| 57 |
self.draw_bot() |
|
| 58 |
self.draw_grid() |
|
| 59 |
|
|
| 60 |
def do_expose_event(self, event): |
|
| 61 |
self.redraw(None) |
|
| 62 |
|
|
| 63 |
def clear_area(self): |
|
| 64 |
self.ctx = self.window.cairo_create() |
|
| 65 |
self.ctx.rectangle(0,0,1000,1000) |
|
| 66 |
self.ctx.set_source_rgb(1,1,1) |
|
| 67 |
self.ctx.fill() |
|
| 68 |
|
|
| 69 |
|
|
| 70 |
def draw_bot(self): |
|
| 71 |
self.ctx = self.window.cairo_create() |
|
| 72 |
self.ctx.new_path() |
|
| 73 |
|
|
| 74 |
x, y, theta = self.draw.bot |
|
| 75 |
y = -y #because we don't want to draw down more units |
|
| 76 |
|
|
| 77 |
#draw blob |
|
| 78 |
self.ctx.set_source_rgb(0,1,0) |
|
| 79 |
xscl = (self.xres - self.xdisp) / self.units |
|
| 80 |
yscl = (self.yres - self.ydisp) / self.units |
|
| 81 |
|
|
| 82 |
cx = x * xscl + self.xdisp |
|
| 83 |
cy = y * yscl + self.ydisp |
|
| 84 |
self.ctx.arc(cx, cy, 20, 0, 2*pi) |
|
| 85 |
self.ctx.fill() |
|
| 86 |
|
|
| 87 |
#draw "arrow" |
|
| 88 |
self.ctx.set_source_rgb(0,0,1) |
|
| 89 |
self.ctx.arc(cx,cy, 5, 0, 2*pi) |
|
| 90 |
self.ctx.fill() |
|
| 91 |
|
|
| 92 |
self.ctx.move_to(cx,cy) |
|
| 93 |
dx = 20 * math.cos(theta + 0.05) |
|
| 94 |
dy = -20 * math.sin(theta + 0.05) |
|
| 95 |
dx = int(dx) |
|
| 96 |
dy = int(dy) |
|
| 97 |
|
|
| 98 |
self.ctx.rel_line_to(dx, dy) |
|
| 99 |
self.ctx.stroke() |
|
| 100 |
|
|
| 101 |
def draw_grid(self): |
|
| 102 |
|
|
| 103 |
self.ctx.set_source_rgb(0,0,0) |
|
| 104 |
self.ctx.new_path() |
|
| 105 |
|
|
| 106 |
#vertical |
|
| 107 |
self.ctx.move_to(400,0) |
|
| 108 |
self.ctx.rel_line_to(0,800) |
|
| 109 |
self.ctx.stroke() |
|
| 110 |
|
|
| 111 |
#vticks |
|
| 112 |
self.ctx.move_to(400,0) |
|
| 113 |
for i in range( self.nticks + 1 ): |
|
| 114 |
self.ctx.rel_line_to(5,0) |
|
| 115 |
self.ctx.stroke() |
|
| 116 |
self.ctx.move_to(400, i * 800 / self.nticks) |
|
| 117 |
|
|
| 118 |
#horiz |
|
| 119 |
self.ctx.move_to(0,400) |
|
| 120 |
self.ctx.rel_line_to(800,0) |
|
| 121 |
self.ctx.stroke() |
|
| 122 |
|
|
| 123 |
#hticks |
|
| 124 |
self.ctx.move_to(0,400) |
|
| 125 |
for i in range(self.nticks + 1): |
|
| 126 |
self.ctx.rel_line_to(0,-5) |
|
| 127 |
self.ctx.stroke() |
|
| 128 |
self.ctx.move_to(i * 800 / self.nticks, 400) |
|
| 129 |
|
|
| 130 |
def draw_graph(self, equations, n): |
|
| 131 |
self.ctx = self.window.cairo_create() |
|
| 132 |
self.ctx.set_source_rgb(0,0,0) |
|
| 133 |
self.ctx.new_path() |
|
| 134 |
|
|
| 135 |
for equn in equations: |
|
| 136 |
xmin, xmax = equn[-2],equn[-1] |
|
| 137 |
eqn = equn[:-2] |
|
| 138 |
dx = float(xmax - xmin) / n |
|
| 139 |
x = xmin |
|
| 140 |
y = 0 |
|
| 141 |
for i in range(len(eqn)): |
|
| 142 |
y += eqn[-1 * i - 1] * ( x ** i) |
|
| 143 |
|
|
| 144 |
self.ctx.move_to(self.x_disp + x * self.scale_const,400 - y * self.scale_const) |
|
| 145 |
|
|
| 146 |
for diffel in range(1,n+1): |
|
| 147 |
x = xmin + diffel * dx |
|
| 148 |
y = 0 |
|
| 149 |
for i in range(len(eqn)): |
|
| 150 |
y += eqn[-1 * i - 1] * ( x ** i) |
|
| 151 |
self.ctx.line_to(self.x_disp + int(x * self.scale_const),400 - int(y * self.scale_const)) |
|
| 152 |
self.ctx.stroke() |
|
| 153 |
self.ctx.move_to(self.x_disp + int(x * self.scale_const),400 - int(y * self.scale_const)) |
|
| 154 |
|
|
| 155 |
def receive_info(self): |
|
| 156 |
|
|
| 157 |
buf = self.socket.recv(100) |
|
| 158 |
if (buf): |
|
| 159 |
items = unpack('4c2hf5h')
|
|
| 160 |
print items |
|
| 161 |
|
|
| 162 |
# info = self.draw.bot |
|
| 163 |
# dx = random() * 2 - 1 |
|
| 164 |
# dy = random() * 2 - 1 |
|
| 165 |
|
|
| 166 |
# self.draw.bot = [info[0] + dx, info[1] + dx, info[2]] |
|
| 167 |
|
|
| 168 |
return True |
|
| 169 |
|
|
| 170 |
|
|
| 171 |
def solve(lisvals): |
|
| 172 |
p0y, p1y, m0y, m1y, x0, x1 = lisvals |
|
| 173 |
A = array([[x0**3 , x0**2 , x0 , 1], |
|
| 174 |
[x1**3 , x1**2 , x1 , 1], |
|
| 175 |
[3 * x0**2 , 2 * x0 , 1 , 0], |
|
| 176 |
[3 * x1**2 , 2 * x1 , 1 , 0]]) |
|
| 177 |
|
|
| 178 |
B = array([p0y, p1y, m0y, m1y]) |
|
| 179 |
tarr = linalg.solve(A,B) |
|
| 180 |
lis = [] |
|
| 181 |
for i in range(4): |
|
| 182 |
lis.append(tarr[i]) |
|
| 183 |
return lis + [0,1] |
|
| 184 |
|
|
| 185 |
def main(): |
|
| 186 |
|
|
| 187 |
window = gtk.Window() |
|
| 188 |
window.connect("delete-event", gtk.main_quit)
|
|
| 189 |
|
|
| 190 |
sock = socket() |
|
| 191 |
sock.connect((hostname, int(sys.argv[1]))) |
|
| 192 |
|
|
| 193 |
# mywidg = Screen(sock) |
|
| 194 |
|
|
| 195 |
mywidg = Screen(sock) |
|
| 196 |
|
|
| 197 |
bigbox = gtk.HBox() |
|
| 198 |
bigbox.pack_end(mywidg, True, True, 0) |
|
| 199 |
|
|
| 200 |
window.add(bigbox) |
|
| 201 |
bigbox.show() |
|
| 202 |
mywidg.show() |
|
| 203 |
window.show() |
|
| 204 |
|
|
| 205 |
gobject.idle_add(mywidg.receive_info) |
|
| 206 |
# gobject.timeout_add(250, mywidg.receive_info) |
|
| 207 |
gtk.main() |
|
| 208 |
|
|
| 209 |
if __name__=='__main__': |
|
| 210 |
if (len(sys.argv) < 2): #or not str.isdecimal(sys.argv[1])): |
|
| 211 |
print("Usage: %s <portno>" % sys.argv[0])
|
|
| 212 |
sys.exit() |
|
| 213 |
|
|
| 214 |
main() |
|
| 0 | 215 | |
| trunk/code/projects/mapping/python/server.c | ||
|---|---|---|
| 22 | 22 |
int main(int argc, char *argv[]) |
| 23 | 23 |
{
|
| 24 | 24 |
char buffer[256]; |
| 25 |
int tmp = 0; |
|
| 25 | 26 |
char outbuf[256]; |
| 26 | 27 |
int n, newsockfd, len; |
| 28 |
short int tprint; |
|
| 27 | 29 |
Packet *received; |
| 28 |
|
|
| 29 |
|
|
| 30 | 30 |
if (argc < 2) {
|
| 31 | 31 |
fprintf(stderr,"usage: %s <port number>\n", argv[0]); |
| 32 | 32 |
exit(1); |
| 33 | 33 |
} |
| 34 | 34 |
newsockfd = make_sock(atoi(argv[1])); |
| 35 |
|
|
| 35 |
printf("Made socket\n");
|
|
| 36 | 36 |
make_listener(); |
| 37 | 37 |
printf("Robot listener created.\n");
|
| 38 | 38 |
|
| ... | ... | |
| 40 | 40 |
|
| 41 | 41 |
while (1) |
| 42 | 42 |
{
|
| 43 |
bzero(buffer, 256); |
|
| 43 | 44 |
wl_do(); |
| 44 | 45 |
|
| 45 |
/* this is a completely useless queue, since wl_do only reads one packet at a time */ |
|
| 46 | 46 |
received = dequeue(); |
| 47 |
/* oh well */ |
|
| 48 | 47 |
|
| 49 | 48 |
if (received) |
| 50 | 49 |
{
|
| 51 |
fprintf(stdout, "len: %d\n", buffer[1]); |
|
| 52 | 50 |
buffer[0] = (char) received->source; |
| 53 |
buffer[1] = (char) received->length-2;
|
|
| 51 |
buffer[1] = (char) received->length; |
|
| 54 | 52 |
buffer[2] = (char) received->type; |
| 55 |
memcpy(buffer+3, received->packet, received->length); |
|
| 53 |
buffer[3] = 0; |
|
| 54 |
memcpy(buffer+4, received->packet, received->length); |
|
| 56 | 55 |
|
| 57 |
n = write(newsockfd, buffer, 200); |
|
| 56 |
memcpy(&tprint, buffer+4, 2); |
|
| 57 |
|
|
| 58 |
printf("%d\n", tprint);
|
|
| 59 |
n = write(newsockfd, buffer, 22); |
|
| 58 | 60 |
free(received); |
| 59 | 61 |
bzero(buffer, 256); |
| 60 | 62 |
} |
| trunk/code/projects/mapping/python/robots.c | ||
|---|---|---|
| 4 | 4 |
#include <stdio.h> |
| 5 | 5 |
#include "robots.h" |
| 6 | 6 |
|
| 7 |
#define CHANNEL 0xE |
|
| 8 |
|
|
| 7 | 9 |
typedef struct Node {
|
| 8 | 10 |
Packet *info; |
| 9 | 11 |
struct Node *next; |
| ... | ... | |
| 18 | 20 |
{
|
| 19 | 21 |
PacketGroupHandler *receiver; |
| 20 | 22 |
printf("wl_init() returned %d\n", wl_init());
|
| 21 |
wl_set_channel(0xE);
|
|
| 23 |
wl_set_channel(CHANNEL);
|
|
| 22 | 24 |
receiver = malloc(sizeof(PacketGroupHandler)); |
| 23 | 25 |
receiver->groupCode = GROUP; |
| 24 | 26 |
receiver->handle_response = NULL; |
| trunk/code/projects/mapping/python/colony.py | ||
|---|---|---|
| 1 | 1 |
#!/usr/bin/env python3.0 |
| 2 | 2 |
import sys, time |
| 3 | 3 |
from socket import * |
| 4 |
from struct import * |
|
| 4 | 5 |
|
| 5 | 6 |
hostname='localhost' |
| 6 | 7 |
|
| ... | ... | |
| 9 | 10 |
|
| 10 | 11 |
def main(): |
| 11 | 12 |
|
| 12 |
funcs = {31 : 'print_range_info'}
|
|
| 13 |
funcs = {1 : 'print_range_info'}
|
|
| 13 | 14 |
|
| 14 | 15 |
buf = [] |
| 15 | 16 |
sock = socket() |
| ... | ... | |
| 26 | 27 |
|
| 27 | 28 |
def print_range_info(packet): |
| 28 | 29 |
print ('Info from %d' % packet[0])
|
| 30 |
|
|
| 29 | 31 |
src = packet[0] |
| 30 |
len = packet[1] |
|
| 31 |
print(len) |
|
| 32 |
data = packet[3:] |
|
| 33 |
for i in range(len // 2): |
|
| 34 |
bot_id = data[i*2] |
|
| 35 |
max_range_id = data[i*2+1] |
|
| 36 |
print ('Max to bot %d is %d' % (bot_id, max_range_id))
|
|
| 37 |
|
|
| 38 | 32 |
|
| 33 |
packet_length = packet[1] |
|
| 39 | 34 |
|
| 35 |
data = packet[3 : 3+packet_length] |
|
| 36 |
print("data length: %d" % len(data))
|
|
| 37 |
ranges = unpack('hhhhhhh',data)
|
|
| 40 | 38 |
|
| 39 |
print(ranges) |
|
| 40 |
|
|
| 41 |
|
|
| 41 | 42 |
if __name__=='__main__': |
| 42 | 43 |
|
| 43 | 44 |
if len(sys.argv) < 2 or not str.isdecimal(sys.argv[1]): |
| trunk/code/projects/mapping/python/Makefile | ||
|---|---|---|
| 2 | 2 |
CFLAGS=-W -Wall -lpthread -g |
| 3 | 3 |
INCS=-I../../libwireless/lib |
| 4 | 4 |
COMPS=../../libwireless/lib |
| 5 |
DEFS=-DWL_DEBUG -DXBEE_SPECIAL |
|
| 5 | 6 |
|
| 6 | 7 |
|
| 7 | 8 |
server : clean |
| 8 |
$(CC) $(CFLAGS) $(INCS) ../../libwireless/lib/*.c server.c netserv.c robots.c -o server |
|
| 9 |
$(CC) $(CFLAGS) $(INCS) $(DEFS) ../../libwireless/lib/*.c server.c netserv.c robots.c -o server
|
|
| 9 | 10 |
|
| 10 | 11 |
clean : |
| 11 | 12 |
rm -f server |
Also available in: Unified diff