Revision 1938
Some bugfixes for collision avoidance.
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