Project

General

Profile

Revision 1938

Some bugfixes for collision avoidance.

View differences:

trunk/code/projects/traffic_navigation/collision_avoid.c
3 3
 *  - Actually Test this stuff. Especially the intersection rotating thing. and the parts marked sketchy.
4 4
 *  - Integrate into main.c
5 5
 *  - In the intersection queue in main.c, allow bots to skip each other (since this code allows it).
6
 *    (because some bot at another intersection entrance may be able to go before another bot at another
7
 *    intersection entrance, but their order in the queue may not allow this.).
6 8
*/
7 9
#include "collision_avoid.h"
8 10
#include "traffic_navigation.h"
......
48 50
	my_path = my_path | D0E;
49 51
	my_path = my_path | I0;
50 52
	char genericNext = nextDir-fromDir;
53
	if(genericNext < 0){
54
		genericNext += 4;
55
	}
51 56
	if(genericNext == 1){ //right turn
52 57
#ifdef DEBUG_CA
53 58
		usb_puts("CA: Setting up Generic Right Turn\n");
......
125 130
	sendBuffer[0] = WCOLLISIONAVOID;
126 131
	sendBuffer[1] = id;
127 132
	sendBuffer[2] = intersection;
128
	sendBuffer[3] = path; /* sketchy */
133
	sendBuffer[3] = path; /* sketchy because path is an int. so will it take both sendBuffer[3] and sendBuffer[4]? */
129 134
	wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
130 135
}
131 136

  
......
145 150
}
146 151

  
147 152
/* Call this when the bot gets a collision avoidance packet that *MATCHES*
148
 * the intersection it is in. This function will return whether it is okay for
153
 * the intersection it is in and the id identifying the sender in the packet
154
 * matches the bot id that is in line in front. (so the collision avoidance
155
 * packet sent by the bot last in the queue gets no listener. This is expected.)
156
 * This function will return whether it is okay for
149 157
 * the bot to proceed into the intersection with a true or false.
150 158
 *
151 159
 * char id: the bot's id
......
154 162
 * What this does is check if the path that the bot needs to travel is clear by
155 163
 * comparing my_path and the path in the packet.
156 164
 * If it is clear, the bot will add my_path to the path it got in the wireless
157
 * packet and then send it. Then it the function will return true.
165
 * packet and then send it. Then the function will return true.
158 166
 * Otherwise, the bot will only fill in the entrance bit in the function it
159 167
 * recieved in the wireless packet to indicate it is sitting at the entrance
160 168
 * to the intersection so that bots behind it won't try to enter the intersection
......
170 178
	if(recieve[2] != intersection)
171 179
		usb_puts("CA ERROR: Recieved Packet's intersection doesn't match the intersection CA is operating on in this Bot.. (CA_E4)\n");
172 180
#endif
173
	if((*(recieve+3) & my_path) != 0){ /* Sketchy Code */
181
	if((*(recieve+3) & my_path) != 0){ /* Sketchy Code because recieve is made of chars and my_path is an int */
174 182
#ifdef DEBUG_CA
175 183
		usb_puts("CA: My Path is occupied through intersection ");
176 184
		usb_puti(intersection);
177 185
		usb_putc('\n');
178 186
#endif
179 187
		result = false;
180
		ca_sendPacket(id, 1 << first_hop | *(recieve+3)); /* Sketchy Code */
188
		ca_sendPacket(id, 1 << first_hop | *(recieve+3)); /* Sketchy Code because recieve is made of chars and ca_send_packet expects an int. we need the whole int to make it through*/
181 189
	}
182 190
	else{
183 191
#ifdef DEBUG_CA
......
186 194
		usb_putc('\n');
187 195
#endif
188 196
		result = true;
189
		ca_sendPacket(id, my_path | *(recieve+3)); /* Sketchy Code */
197
		ca_sendPacket(id, my_path | *(recieve+3)); /* Sketchy Code because recieve is made of chars and ca_send_packet expects an int.  we need the whole int to make it through. Also, my_path is an int.*/
190 198
	}
191 199
	return result;
192 200
}

Also available in: Unified diff