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