Revision 932
| trunk/code/projects/mapping/python/map.py (revision 932) | ||
|---|---|---|
| 38 | 38 |
|
| 39 | 39 |
self.set_size_request(self.xres, self.yres) |
| 40 | 40 |
|
| 41 |
|
|
| 42 | 41 |
self.socket = socket |
| 43 |
self.bots = [] |
|
| 42 |
self.bots = [i for i in xrange(16)] |
|
| 44 | 43 |
|
| 45 | 44 |
self.draw = map_model() |
| 46 |
self.draw.bot = [1,1,pi/2] # x y theta |
|
| 45 |
self.draw.bot = [[1,1,pi/2],0] # x y theta |
|
| 47 | 46 |
self.draw.add_dependent(self.redraw) |
| 48 | 47 |
|
| 49 | 48 |
self.scale_const = 50 |
| ... | ... | |
| 71 | 70 |
self.ctx = self.window.cairo_create() |
| 72 | 71 |
self.ctx.new_path() |
| 73 | 72 |
|
| 74 |
x, y, theta = self.draw.bot |
|
| 73 |
tbot = self.draw.bot |
|
| 74 |
print tbot |
|
| 75 |
x, y, theta = tbot[:3] |
|
| 75 | 76 |
y = -y #because we don't want to draw down more units |
| 76 | 77 |
|
| 77 | 78 |
#draw blob |
| ... | ... | |
| 156 | 157 |
|
| 157 | 158 |
buf = self.socket.recv(100) |
| 158 | 159 |
if (buf): |
| 159 |
items = unpack('4c2hf5h')
|
|
| 160 |
print items |
|
| 160 |
items = unpack('4c2hf5h',buf)
|
|
| 161 |
src = ord(items[0]) |
|
| 162 |
old = self.draw.bot #the info of bot src at prev iter |
|
| 163 |
newx = items[4] / 30.0 |
|
| 164 |
newy = items[5] / 30.0 |
|
| 165 |
new = [newx, newy] + [items[i] for i in range(6,12)] |
|
| 166 |
self.draw.bot = new |
|
| 161 | 167 |
|
| 162 | 168 |
# info = self.draw.bot |
| 163 | 169 |
# dx = random() * 2 - 1 |
| 164 | 170 |
# dy = random() * 2 - 1 |
| 165 |
|
|
| 171 |
|
|
| 166 | 172 |
# self.draw.bot = [info[0] + dx, info[1] + dx, info[2]] |
| 167 | 173 |
|
| 168 | 174 |
return True |
| trunk/code/projects/mapping/python/server.c (revision 932) | ||
|---|---|---|
| 22 | 22 |
int main(int argc, char *argv[]) |
| 23 | 23 |
{
|
| 24 | 24 |
char buffer[256]; |
| 25 |
int tmp = 0; |
|
| 26 |
char outbuf[256]; |
|
| 27 |
int n, newsockfd, len; |
|
| 28 |
short int tprint; |
|
| 25 |
int n, newsockfd; |
|
| 26 |
short int x,y,ir1,ir2,ir3,ir4,ir5; |
|
| 27 |
float theta; |
|
| 29 | 28 |
Packet *received; |
| 30 | 29 |
if (argc < 2) {
|
| 31 | 30 |
fprintf(stderr,"usage: %s <port number>\n", argv[0]); |
| 32 | 31 |
exit(1); |
| 33 | 32 |
} |
| 34 |
newsockfd = make_sock(atoi(argv[1])); |
|
| 33 |
/* newsockfd = make_sock(atoi(argv[1])); */ |
|
| 35 | 34 |
printf("Made socket\n");
|
| 36 | 35 |
make_listener(); |
| 37 | 36 |
printf("Robot listener created.\n");
|
| ... | ... | |
| 53 | 52 |
buffer[3] = 0; |
| 54 | 53 |
memcpy(buffer+4, received->packet, received->length); |
| 55 | 54 |
|
| 56 |
memcpy(&tprint, buffer+4, 2); |
|
| 55 |
memcpy(&x, buffer+4, 2); |
|
| 56 |
memcpy(&y, buffer+6, 2); |
|
| 57 |
memcpy(&theta, buffer+8, 4); |
|
| 58 |
memcpy(&ir1, buffer+12, 2); |
|
| 59 |
memcpy(&ir2, buffer+14, 2); |
|
| 60 |
memcpy(&ir3, buffer+16, 2); |
|
| 61 |
memcpy(&ir4, buffer+18, 2); |
|
| 62 |
memcpy(&ir5, buffer+20, 2); |
|
| 57 | 63 |
|
| 58 |
printf("%d\n", tprint);
|
|
| 59 |
n = write(newsockfd, buffer, 22); |
|
| 64 |
printf("(%d, %d, %f, %d, %d, %d, %d, %d)\n",
|
|
| 65 |
x, y, theta, ir1, ir2, ir3, ir4, ir5); |
|
| 66 |
|
|
| 67 |
/* n = write(newsockfd, buffer, 22); */ |
|
| 60 | 68 |
free(received); |
| 61 | 69 |
bzero(buffer, 256); |
| 62 | 70 |
} |
| trunk/code/projects/mapping/python/robots.c (revision 932) | ||
|---|---|---|
| 1 | 1 |
#include <stdlib.h> |
| 2 | 2 |
#include <wireless.h> |
| 3 |
#include <wl_token_ring.h> |
|
| 3 | 4 |
#include <string.h> |
| 4 | 5 |
#include <stdio.h> |
| 5 | 6 |
#include "robots.h" |
| 6 | 7 |
|
| 7 | 8 |
#define CHANNEL 0xE |
| 9 |
#define GROUP 1 |
|
| 8 | 10 |
|
| 9 | 11 |
typedef struct Node {
|
| 10 | 12 |
Packet *info; |
| ... | ... | |
| 15 | 17 |
Node *thead = NULL; |
| 16 | 18 |
|
| 17 | 19 |
void enqueue(char typ, int src, unsigned char *wrds,int len); |
| 20 |
void do_nothing(void); |
|
| 18 | 21 |
|
| 19 | 22 |
void make_listener() |
| 20 | 23 |
{
|
| ... | ... | |
| 23 | 26 |
wl_set_channel(CHANNEL); |
| 24 | 27 |
receiver = malloc(sizeof(PacketGroupHandler)); |
| 25 | 28 |
receiver->groupCode = GROUP; |
| 26 |
receiver->handle_response = NULL; |
|
| 27 |
receiver->timeout_handler = NULL; |
|
| 29 |
receiver->handle_response = do_nothing; |
|
| 30 |
receiver->timeout_handler = do_nothing; |
|
| 28 | 31 |
receiver->handle_receive = enqueue; |
| 29 | 32 |
wl_register_packet_group(receiver); |
| 30 | 33 |
} |
| 31 | 34 |
|
| 32 |
|
|
| 35 |
//if we ever get more than one packet, before reading any |
|
| 33 | 36 |
void enqueue(char typ, int src, unsigned char *wrds,int len) |
| 34 | 37 |
{
|
| 38 |
printf("queued one\n");
|
|
| 35 | 39 |
Node *new = malloc(sizeof(Node)); |
| 36 | 40 |
new->info = malloc(sizeof(Packet)); |
| 37 | 41 |
new->info->source = src; |
| ... | ... | |
| 66 | 70 |
free (iter); |
| 67 | 71 |
return ret; |
| 68 | 72 |
} |
| 73 |
|
|
| 74 |
void do_nothing(void) |
|
| 75 |
{
|
|
| 76 |
return; |
|
| 77 |
} |
|
| trunk/code/projects/mapping/python/Makefile (revision 932) | ||
|---|---|---|
| 1 | 1 |
CC=gcc |
| 2 |
CFLAGS=-W -Wall -lpthread -g |
|
| 2 |
CFLAGS=-W -Wall -lpthread |
|
| 3 | 3 |
INCS=-I../../libwireless/lib |
| 4 | 4 |
COMPS=../../libwireless/lib |
| 5 | 5 |
DEFS=-DWL_DEBUG -DXBEE_SPECIAL |
| trunk/code/projects/mapping/python/robots.h (revision 932) | ||
|---|---|---|
| 9 | 9 |
char *packet; |
| 10 | 10 |
} Packet; |
| 11 | 11 |
|
| 12 |
#define GROUP 1 |
|
| 13 | 12 |
|
| 14 | 13 |
void make_listener(); |
| 15 | 14 |
Packet *dequeue(); |
Also available in: Unified diff