Revision 934
| trunk/code/projects/mapping/python/map.py (revision 934) | ||
|---|---|---|
| 165 | 165 |
new = [newx, newy] + [items[i] for i in range(6,12)] |
| 166 | 166 |
self.draw.bot = new |
| 167 | 167 |
|
| 168 |
# info = self.draw.bot |
|
| 169 |
# dx = random() * 2 - 1 |
|
| 170 |
# dy = random() * 2 - 1 |
|
| 171 |
|
|
| 172 |
# self.draw.bot = [info[0] + dx, info[1] + dx, info[2]] |
|
| 173 |
|
|
| 174 | 168 |
return True |
| 175 | 169 |
|
| 176 | 170 |
|
| ... | ... | |
| 195 | 189 |
|
| 196 | 190 |
sock = socket() |
| 197 | 191 |
sock.connect((hostname, int(sys.argv[1]))) |
| 198 |
|
|
| 199 |
# mywidg = Screen(sock) |
|
| 200 | 192 |
|
| 201 | 193 |
mywidg = Screen(sock) |
| 202 | 194 |
|
| ... | ... | |
| 209 | 201 |
window.show() |
| 210 | 202 |
|
| 211 | 203 |
gobject.idle_add(mywidg.receive_info) |
| 212 |
# gobject.timeout_add(250, mywidg.receive_info) |
|
| 213 | 204 |
gtk.main() |
| 214 | 205 |
|
| 215 | 206 |
if __name__=='__main__': |
| trunk/code/projects/mapping/python/server.c (revision 934) | ||
|---|---|---|
| 13 | 13 |
#include "robots.h" |
| 14 | 14 |
#include "wireless.h" |
| 15 | 15 |
|
| 16 |
void donothin() |
|
| 17 |
{
|
|
| 18 |
return; |
|
| 19 |
} |
|
| 20 | 16 |
|
| 21 | 17 |
|
| 22 | 18 |
int main(int argc, char *argv[]) |
| 23 | 19 |
{
|
| 24 |
char buffer[256]; |
|
| 25 |
int n, newsockfd; |
|
| 26 |
short int x,y,ir1,ir2,ir3,ir4,ir5; |
|
| 27 |
float theta; |
|
| 28 |
Packet *received; |
|
| 29 | 20 |
if (argc < 2) {
|
| 30 | 21 |
fprintf(stderr,"usage: %s <port number>\n", argv[0]); |
| 31 | 22 |
exit(1); |
| 32 | 23 |
} |
| 33 |
/* newsockfd = make_sock(atoi(argv[1])); */ |
|
| 24 |
newsockfd = make_sock(atoi(argv[1])); |
|
| 34 | 25 |
printf("Made socket\n");
|
| 35 | 26 |
make_listener(); |
| 36 | 27 |
printf("Robot listener created.\n");
|
| 37 |
|
|
| 38 |
bzero(buffer,256); |
|
| 39 |
|
|
| 28 |
|
|
| 40 | 29 |
while (1) |
| 41 | 30 |
{
|
| 42 |
bzero(buffer, 256); |
|
| 43 | 31 |
wl_do(); |
| 44 |
|
|
| 45 |
received = dequeue(); |
|
| 46 |
|
|
| 47 |
if (received) |
|
| 48 |
{
|
|
| 49 |
buffer[0] = (char) received->source; |
|
| 50 |
buffer[1] = (char) received->length; |
|
| 51 |
buffer[2] = (char) received->type; |
|
| 52 |
buffer[3] = 0; |
|
| 53 |
memcpy(buffer+4, received->packet, received->length); |
|
| 54 |
|
|
| 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); |
|
| 63 |
|
|
| 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); */ |
|
| 68 |
free(received); |
|
| 69 |
bzero(buffer, 256); |
|
| 70 |
} |
|
| 71 | 32 |
} |
| 72 | 33 |
return 0; |
| 73 | 34 |
} |
| ... | ... | |
| 77 | 38 |
printf(msg); |
| 78 | 39 |
exit(1); |
| 79 | 40 |
} |
| 41 |
|
|
| trunk/code/projects/mapping/python/robots.c (revision 934) | ||
|---|---|---|
| 3 | 3 |
#include <wl_token_ring.h> |
| 4 | 4 |
#include <string.h> |
| 5 | 5 |
#include <stdio.h> |
| 6 |
#include <unistd.h> |
|
| 6 | 7 |
#include "robots.h" |
| 7 | 8 |
|
| 8 | 9 |
#define CHANNEL 0xE |
| 9 | 10 |
#define GROUP 1 |
| 10 | 11 |
|
| 11 |
typedef struct Node {
|
|
| 12 |
Packet *info; |
|
| 13 |
struct Node *next; |
|
| 14 |
struct Node *prev; |
|
| 15 |
} Node; |
|
| 16 |
|
|
| 17 |
Node *thead = NULL; |
|
| 18 |
|
|
| 19 |
void enqueue(char typ, int src, unsigned char *wrds,int len); |
|
| 20 | 12 |
void do_nothing(void); |
| 13 |
void packet_receive(char type, int source, unsigned char* packet, int length); |
|
| 21 | 14 |
|
| 22 | 15 |
void make_listener() |
| 23 | 16 |
{
|
| ... | ... | |
| 28 | 21 |
receiver->groupCode = GROUP; |
| 29 | 22 |
receiver->handle_response = do_nothing; |
| 30 | 23 |
receiver->timeout_handler = do_nothing; |
| 31 |
receiver->handle_receive = enqueue; |
|
| 24 |
receiver->handle_receive = packet_receive; |
|
| 32 | 25 |
wl_register_packet_group(receiver); |
| 33 | 26 |
} |
| 34 | 27 |
|
| 35 |
//if we ever get more than one packet, before reading any |
|
| 36 |
void enqueue(char typ, int src, unsigned char *wrds,int len) |
|
| 28 |
void packet_receive(char type, int source, unsigned char* packet, int length) |
|
| 37 | 29 |
{
|
| 38 |
printf("queued one\n");
|
|
| 39 |
Node *new = malloc(sizeof(Node)); |
|
| 40 |
new->info = malloc(sizeof(Packet)); |
|
| 41 |
new->info->source = src; |
|
| 42 |
new->info->length = len; |
|
| 43 |
new->info->type = typ; |
|
| 44 |
new->info->packet = malloc((strlen(wrds)+1)*sizeof(char)); |
|
| 45 |
strcpy(new->info->packet, wrds); |
|
| 30 |
/* stolen from cmar */ |
|
| 31 |
char buffer[22]; |
|
| 32 |
short x,y,ir1,ir2,ir3,ir4,ir5, n; |
|
| 33 |
x = ((short)packet[1] << 8) | (short)packet[0]; |
|
| 34 |
y = ((short)packet[3] << 8) | (short)packet[2]; |
|
| 35 |
ir1 = ((short)packet[9] << 8) | (short)packet[8]; |
|
| 36 |
ir2 = ((short)packet[11] << 8) | (short)packet[10]; |
|
| 37 |
ir3 = ((short)packet[13] << 8) | (short)packet[12]; |
|
| 38 |
ir4 = ((short)packet[15] << 8) | (short)packet[14]; |
|
| 39 |
ir5 = ((short)packet[17] << 8) | (short)packet[16]; |
|
| 46 | 40 |
|
| 47 |
if (!thead) |
|
| 48 |
thead = new; |
|
| 49 |
else |
|
| 50 |
{
|
|
| 51 |
new->next = thead; |
|
| 52 |
thead->prev = new; |
|
| 53 |
thead = new; |
|
| 54 |
} |
|
| 41 |
char tarr[] = {packet[4],packet[5],packet[6],packet[7]};
|
|
| 42 |
float *theta_ptr = (float *)tarr; |
|
| 43 |
float theta = *theta_ptr; |
|
| 44 |
|
|
| 45 |
printf( "%d %d %f %d %d %d %d %d\n", x, y, theta, |
|
| 46 |
ir1, ir2, ir3, ir4, ir5); |
|
| 47 |
buffer[0] = (char) source; |
|
| 48 |
buffer[1] = (char) length; |
|
| 49 |
buffer[2] = type; |
|
| 50 |
buffer[3] = 0; |
|
| 51 |
memcpy ( buffer+4, packet, length); |
|
| 52 |
|
|
| 53 |
n = write(newsockfd, buffer, 22); |
|
| 54 |
bzero(buffer, 22); |
|
| 55 |
|
|
| 55 | 56 |
} |
| 56 | 57 |
|
| 57 |
Packet * dequeue() |
|
| 58 |
void do_nothing() |
|
| 58 | 59 |
{
|
| 59 |
Node *iter; |
|
| 60 |
Packet *ret; |
|
| 61 |
if (!thead) |
|
| 62 |
return NULL; |
|
| 63 |
for (iter = thead; iter->next != NULL; iter = iter->next) |
|
| 64 |
; |
|
| 65 |
ret = iter->info; |
|
| 66 |
if (iter->prev) |
|
| 67 |
(iter->prev)->next = NULL; |
|
| 68 |
else |
|
| 69 |
thead = NULL; |
|
| 70 |
free (iter); |
|
| 71 |
return ret; |
|
| 72 |
} |
|
| 73 |
|
|
| 74 |
void do_nothing(void) |
|
| 75 |
{
|
|
| 76 | 60 |
return; |
| 77 | 61 |
} |
| trunk/code/projects/mapping/python/robots.h (revision 934) | ||
|---|---|---|
| 1 | 1 |
#ifndef ROBOTS |
| 2 | 2 |
#define ROBOTS |
| 3 | 3 |
|
| 4 |
typedef struct Packet |
|
| 5 |
{
|
|
| 6 |
int source; |
|
| 7 |
int length; |
|
| 8 |
char type; |
|
| 9 |
char *packet; |
|
| 10 |
} Packet; |
|
| 11 | 4 |
|
| 12 |
|
|
| 13 | 5 |
void make_listener(); |
| 14 |
Packet *dequeue(); |
|
| 6 |
int newsockfd; |
|
| 15 | 7 |
|
| 16 | 8 |
#endif |
Also available in: Unified diff