Project

General

Profile

Revision 966

Small changes - new file just waits for packets to buffer
for matlab, so the robots will decide when to send their
data in this version.

View differences:

trunk/code/projects/mapping/matlab/receive.c
1
/* This version of receive initializes
2
 * the token ring and then requests and
3
 * receives data in a different packet group
4
 * unsynchronized with the token ring.
5
 */
6

  
1 7
#include <stdio.h>
2 8
#include <stdlib.h>
3 9
#include <mex.h>
......
36 42
int head_index; /*Points to the oldest data in the buffers.*/
37 43

  
38 44
void* buffer_data(void* arg);
45
void init_wireless(void);
39 46

  
40 47
void mexFunction(int nlhs, mxArray* ret[], int nrhs, const mxArray* args[]){
41 48
	
......
122 129
	pthread_rwlock_unlock(&buffer_lock);
123 130
}
124 131

  
132
void init_wireless(void){
133
	/*General communication initialization.	*/
134
	wl_set_com_port(WL_COM_PORT);	
135
	wl_init();
136
	wl_set_channel(WL_CHANNEL);
137
	wl_register_packet_group(&requestHandler);
138

  
139
	/*Token ring / sensor network initialization.*/
140
	wl_token_ring_register();	
141
}
142

  
125 143
/*
126 144
 * buffer_data continuously runs through the robots and
127 145
 * waits for them to respond.
......
131 149

  
132 150
	pthread_detach(pthread_self());
133 151
	
134
	/*General communication initialization.	*/
135
	wl_set_com_port(WL_COM_PORT);	
136
	wl_init();
137
	wl_set_channel(WL_CHANNEL);
138
	wl_register_packet_group(&requestHandler);
139

  
140
	/*Token ring / sensor network initialization.*/
141
	wl_token_ring_register();	
152
	init_wireless();
142 153
	
143 154
	/*Continuously iterate over all the robots - buffering data.*/
144 155
	
trunk/code/projects/mapping/matlab/receive2.c
1
#include <stdio.h>
2
#include <stdlib.h>
3
#include <mex.h>
4
#include <pthread.h>
5
#include <unistd.h>
6
#include "../../libwireless/lib/wireless.h"
7
#include "../../libwireless/lib/wl_token_ring.h"
8

  
9
#define WL_COM_PORT "/dev/ttyUSB1" 
10
#define WL_CHANNEL 0xE
11

  
12
#define MAP_REQUEST_GROUP 23
13

  
14
#define DATA_POINT 2 /* packet type for map data points w/ odometry data*/
15
#define DATA_REQUEST 3
16

  
17
#define BUFFER_SIZE 1000
18
#define BUFFER_EMPTY -1
19

  
20
pthread_rwlock_t buffer_lock;
21
pthread_t receive_thread;
22

  
23
int packets_received; /*Used to block overlapping packet requests.*/
24

  
25
void packet_receive(char type, int source, unsigned char* packet, int length);
26

  
27
PacketGroupHandler requestHandler = {MAP_REQUEST_GROUP, NULL, NULL, &packet_receive, NULL};
28

  
29
/*Buffers*/
30
short robot_buffer[BUFFER_SIZE];			/*by #*/
31
short range_buffer[BUFFER_SIZE][5]; 		/*IR1, IR2 ... */
32
short coordinate_buffer[BUFFER_SIZE][2];	/*X Y*/
33
float orientation_buffer[BUFFER_SIZE];
34

  
35
int tail_index; /*Points to the newest data in the buffers.*/
36
int head_index; /*Points to the oldest data in the buffers.*/
37

  
38
void init_wireless(void);
39
void* run_wireless(void*);
40

  
41
void mexFunction(int nlhs, mxArray* ret[], int nrhs, const mxArray* args[]){
42
	
43
	if(nrhs == 1){	
44
		
45
		/*Initialization*/
46
		init_wireless();
47
		
48
		pthread_rwlock_init(&buffer_lock, NULL);
49
		pthread_create(&receive_thread, NULL, run_wireless, NULL);
50
		
51
		return;
52
	}
53
	
54
	/*Return stuff to matlab*/
55
	/*deliberately ignore nrhs.	*/
56

  
57
	if(nlhs != 4){
58
		mexErrMsgTxt("Error: Expect output arguments [P IR T R]\n  \
59
					  where P = [X Y], IR = [IR1, IR2 ...],\n \
60
					  T = angle, and R = Robot number."); 
61
	}
62
	
63
	int index,i,j;
64
	
65
	pthread_rwlock_rdlock(&buffer_lock);
66
	{
67

  
68
		if(tail_index == BUFFER_EMPTY || head_index == BUFFER_EMPTY){
69
			double* point, *ranges, *theta, *robot;
70
			
71
			/*Appropriately return with nothing. (all 0's)*/
72
			ret[0] = mxCreateDoubleMatrix(1, 2, mxREAL);
73
			ret[1] = mxCreateDoubleMatrix(1, 5, mxREAL);
74
			ret[2] = mxCreateDoubleMatrix(1, 1, mxREAL);
75
			ret[3] = mxCreateDoubleMatrix(1, 1, mxREAL);
76
			
77
			point  = mxGetPr(ret[0]);
78
			ranges = mxGetPr(ret[1]);
79
			theta = mxGetPr(ret[2]);
80
			robot = mxGetPr(ret[3]);
81

  
82
			point[0] = point[1] = 0.0;
83
			
84
			ranges[0] = ranges[1] = ranges[2] = 0.0;
85
			ranges[3] = ranges[4] = 0.0;
86

  
87
			theta[0] = 0.0;
88
			robot[0] = 0.0;
89

  
90
			return;
91
		}
92

  
93
		else{
94
			double** point, **ranges, *theta, *robot;
95
			
96
			int buf_count, num_buffer_elements = tail_index - head_index;
97
			if(num_buffer_elements < 0) num_buffer_elements += BUFFER_SIZE;	
98

  
99
			ret[0] = mxCreateDoubleMatrix(num_buffer_elements, 2, mxREAL);
100
			ret[1] = mxCreateDoubleMatrix(num_buffer_elements, 5, mxREAL);
101
			ret[2] = mxCreateDoubleMatrix(num_buffer_elements, 1, mxREAL);
102
			ret[3] = mxCreateDoubleMatrix(num_buffer_elements, 1, mxREAL);
103
		
104
			/*This may not work on some computers.*/
105
			point  = (double**)mxGetPr(ret[0]);
106
			ranges = (double**)mxGetPr(ret[1]);
107
			theta = mxGetPr(ret[2]);
108
			robot = mxGetPr(ret[3]);
109
			
110
			for(index = head_index, buf_count = 0; buf_count < num_buffer_elements; 
111
				buf_count++, index = (index + 1)%1024){
112
				
113
				point[i][0] = (double)coordinate_buffer[index][0];	
114
				point[i][1] = (double)coordinate_buffer[index][1];	
115
				
116
				for(j=0;j<5;j++)
117
					ranges[i][j] = (double)range_buffer[index][j];
118

  
119
				theta[i] = (double)(orientation_buffer[index]);	
120
				robot[i] = (double)(robot_buffer[index]);
121
			}
122

  
123
			head_index = tail_index = BUFFER_EMPTY;
124
		}
125
	}	
126
	pthread_rwlock_unlock(&buffer_lock);
127
}
128

  
129
void init_wireless(void){
130
	/*General communication initialization.	*/
131
	wl_set_com_port(WL_COM_PORT);	
132
	wl_init();
133
	wl_set_channel(WL_CHANNEL);
134
	wl_register_packet_group(&requestHandler);
135

  
136
	/*Token ring / sensor network initialization.*/
137
	wl_token_ring_register();	
138
}
139

  
140
void* run_wireless(void* arg){
141
	pthread_detach(pthread_self());
142
	while(1){
143
		wl_do();
144
		usleep(5000);
145
	}
146
}
147

  
148
void packet_receive (char type, int source, unsigned char* packet, int length) {
149
	printf("A packet came in!\n");	
150
	if(type==DATA_POINT){	
151

  
152
		if(length!=18) printf("Packet came without the standard length!\n");
153

  
154
		/* expected input: x y theta(double) IR1 IR2 IR3 IR4 IR5*/
155
		/* Pull in data:*/
156
		
157
		short x,y,i;
158
		short ranges[5];	
159
		
160
		x = ((short)packet[1] << 8)  | (short)packet[0];
161
		y = ((short)packet[3] << 8)  | (short)packet[2];
162

  
163
		for(i=0; i<5; i++){
164
			ranges[i] 
165
			   = ((short)packet[8+2*i+1] << 8) | (short)packet[8 + 2*i];
166
		}
167

  
168
		float theta = *((float*)(packet + 4));
169
	
170
		/*Write new data to buffers. (Lock down!)*/
171
		pthread_rwlock_wrlock(&buffer_lock);
172
		{	
173
			/*Note that you write no matter what.	*/
174
			tail_index = (tail_index + 1) % BUFFER_SIZE; 
175
			
176
			/*Write the robots number.*/
177
			robot_buffer[tail_index] = source;
178
			
179
			/*Write in range data.*/
180
			for(i=0; i<5; i++) range_buffer[tail_index][i] = ranges[i];
181
			
182
			coordinate_buffer[tail_index][0] = x;
183
			coordinate_buffer[tail_index][1] = y;
184
			
185
			orientation_buffer[tail_index] = theta;
186
			
187
			/*If the oldest member didn't exist, buffer was empty.*/
188
			if(head_index == BUFFER_EMPTY) head_index = 0;
189
		
190
			/*Otherwise check for overwritten data.	*/
191
			else if(tail_index == head_index) 
192
				head_index = (head_index +1) % BUFFER_SIZE;  
193
			
194
		}
195
		pthread_rwlock_unlock(&buffer_lock);
196
		
197
		/*Increment for the transmit thread.*/
198
		packets_received++;
199
	}
200
}
201

  
trunk/code/projects/mapping/matlab/Makefile
2 2
CFLAGS=
3 3
LIBS= -lpthread -lwireless
4 4

  
5
all: receive
5
all: receive receive2
6 6

  
7 7
test: *.c ../../libwireless/lib/*.c 
8 8
	$(CC) $(CFLAGS) *.c -L../../../lib/bin -DWL_DEBUG $(LIBS)
......
10 10
receive:
11 11
	 mex -g receive.c -L../../libwireless/lib -lwireless
12 12

  
13
receive2:
14
	 mex -g receive2.c -L../../libwireless/lib -lwireless
15

  
13 16
clean:
14 17
	rm *.o 
15 18

  

Also available in: Unified diff