root / trunk / code / projects / traffic_navigation / collision_avoid.c @ 1929
History | View | Annotate | Download (6.07 KB)
1 |
/* TODO:
|
---|---|
2 |
* - Document how to call functions.
|
3 |
* - Actually Test this stuff. Especially the intersection rotating thing. and the parts marked sketchy.
|
4 |
* - Integrate into main.c
|
5 |
* - In the intersection queue in main.c, allow bots to skip each other (since this code allows it).
|
6 |
*/
|
7 |
#include "collision_avoid.h" |
8 |
#include "traffic_navigation.h" |
9 |
static unsigned int my_path = 0; |
10 |
static unsigned int first_hop = 0; |
11 |
static char intersection = 0; |
12 |
|
13 |
/* Dir0
|
14 |
* [0 ][4 ]
|
15 |
*Dir1 [5][8 ][9 ][3] Dir3
|
16 |
* [1][11][10][7]
|
17 |
* [6 ][2 ]
|
18 |
* Dir2
|
19 |
*
|
20 |
* Dir0
|
21 |
* [D0E][D0X]
|
22 |
*Dir1 [D1X][I0][I1][D3E] Dir3
|
23 |
* [D1E][12][13][D3X]
|
24 |
* [D2X][D2E]
|
25 |
* Dir2
|
26 |
*/
|
27 |
|
28 |
/* Upon entering an intersection, this function should be called. To achieve
|
29 |
* nonstop motion when possible, it is preferable to call this right after
|
30 |
* reading the barcode.
|
31 |
*
|
32 |
* This function sets up the "my_path" variable with the proper bits representing
|
33 |
* the path that the bot will travel in the intersection.
|
34 |
*
|
35 |
* char intersectionNum: The # of the intersection
|
36 |
* char fromDir: The direction the bot is entering the intersection
|
37 |
* char nextDir: The direction the bot is exiting the intersection
|
38 |
*/
|
39 |
void ca_EnterIntersection(char intersectionNum, char fromDir, char nextDir){ |
40 |
/* Generic turn model condenses all turns into either:
|
41 |
* - a right turn from dir0 to dir1
|
42 |
* - a straight from dir0 to dir2
|
43 |
* - a left turn dir0 to dir3
|
44 |
* - maybe a U-turn from dir0 to dir0 **UNIMPLEMENTED*
|
45 |
*/
|
46 |
/* Fill in dir0 */
|
47 |
my_path = 0;
|
48 |
my_path = my_path | D0E; |
49 |
my_path = my_path | I0; |
50 |
char genericNext = nextDir-fromDir;
|
51 |
if(genericNext == 1){ //right turn |
52 |
#ifdef DEBUG_CA
|
53 |
usb_puts("CA: Setting up Generic Right Turn\n");
|
54 |
#endif
|
55 |
my_path = my_path | D1X; |
56 |
} |
57 |
else if(genericNext == 2){ //straight turn |
58 |
#ifdef DEBUG_CA
|
59 |
usb_puts("CA: Setting up Generic Straight Turn\n");
|
60 |
#endif
|
61 |
my_path = my_path | D2X; |
62 |
my_path = my_path | I2; |
63 |
} |
64 |
else{ //left turn |
65 |
#ifdef DEBUG_CA
|
66 |
if(genericNext != 3) |
67 |
usb_puts("CA ERROR: Error in generic turn generator (CA_E0)\n");
|
68 |
usb_puts("CA: Setting up Generic Left Turn\n");
|
69 |
#endif
|
70 |
my_path = my_path | D3X; |
71 |
my_path = my_path | I2; |
72 |
my_path = my_path | I3; |
73 |
|
74 |
} |
75 |
//now rotate turn to match what it's really supposed to be
|
76 |
for(int i = 0; i < fromDir; i++){ |
77 |
int new_path = 0; |
78 |
char temp = my_path & DAE;
|
79 |
char shiftoff = temp & 0x8; |
80 |
temp = temp << 1;
|
81 |
temp = temp | (shiftoff >> 3);
|
82 |
new_path = new_path | temp; |
83 |
|
84 |
temp = (my_path & DAX) >> DAXS; |
85 |
shiftoff = temp & 0x1;
|
86 |
temp = temp << 1;
|
87 |
temp = temp | (shiftoff >> 3);
|
88 |
new_path = new_path | (temp << DAXS); |
89 |
|
90 |
temp = (my_path & DAX) >> IAS; |
91 |
shiftoff = temp & 0x1;
|
92 |
temp = temp << 1;
|
93 |
temp = temp | (shiftoff >> 3);
|
94 |
new_path = new_path | (temp << IAS); |
95 |
my_path = new_path; |
96 |
|
97 |
} |
98 |
first_hop = fromDir; |
99 |
intersection = intersectionNum; |
100 |
} |
101 |
|
102 |
/* Call this function when the bot has left the intersection.
|
103 |
*
|
104 |
* All this function does is clear some variables. I guess
|
105 |
* this is optional but it is nice for debugging. */
|
106 |
void ca_ExitIntersection(){
|
107 |
my_path = 0;
|
108 |
first_hop = -1;
|
109 |
intersection = -1;
|
110 |
} |
111 |
|
112 |
/* Don't call this function. This is strictly an internal function to this library.
|
113 |
*
|
114 |
* int path: Takes the path to be sent in the packet.
|
115 |
*
|
116 |
* This will send a collision avoidance packet with the bot id, the intersection # and path */
|
117 |
void ca_sendPacket(char id, int path){ |
118 |
if(path & D0E && path & D1E && path & D2E && path & D3E){
|
119 |
#ifdef DEBUG_CA
|
120 |
usb_puts("CA: No open entry ways to intersection, so not sending CA packet.");
|
121 |
#endif
|
122 |
return;
|
123 |
} |
124 |
char sendBuffer[PACKET_LENGTH];
|
125 |
sendBuffer[0] = WCOLLISIONAVOID;
|
126 |
sendBuffer[1] = id;
|
127 |
sendBuffer[2] = intersection;
|
128 |
sendBuffer[3] = path; /* sketchy */ |
129 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
|
130 |
} |
131 |
|
132 |
/* Call this if the bot is the first in the queue for the intersection and wishes
|
133 |
* to attempt collision avoidance.
|
134 |
*
|
135 |
* char id: the bot's id.
|
136 |
*
|
137 |
* What this does is send a collision avoidance packet with this bot's path.
|
138 |
*/
|
139 |
void ca_Initiator(char id){ //args: (This bot's ID) |
140 |
#ifdef DEBUG_CA
|
141 |
if(mypath == 0) |
142 |
usb_puts("CA ERROR: Someone Forgot to call ca_EnterIntersection. mypath == 0 (CA_E1)\n");
|
143 |
#endif
|
144 |
ca_sendPacket(id, my_path); |
145 |
} |
146 |
|
147 |
/* 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
|
149 |
* the bot to proceed into the intersection with a true or false.
|
150 |
*
|
151 |
* char id: the bot's id
|
152 |
* char recieve[]: the wireless packet recieved.
|
153 |
*
|
154 |
* What this does is check if the path that the bot needs to travel is clear by
|
155 |
* comparing my_path and the path in the packet.
|
156 |
* 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.
|
158 |
* Otherwise, the bot will only fill in the entrance bit in the function it
|
159 |
* recieved in the wireless packet to indicate it is sitting at the entrance
|
160 |
* to the intersection so that bots behind it won't try to enter the intersection
|
161 |
* and crash into the bot at the front of the line for that intersection.
|
162 |
*/
|
163 |
bool ca_Process(char id, char recieve[]){ //args: (This bot's ID, a pointer to the packet recieved) |
164 |
bool result = false; |
165 |
#ifdef DEBUG_CA
|
166 |
if(recieve[0] != WCOLLISIONAVOID) |
167 |
usb_puts("CA ERROR: You didn't call ca_Process with a WCOLLISIONAVOID type packet (CA_E2)\n");
|
168 |
if(recieve[1] < 0) |
169 |
usb_puts("CA ERROR: Recieved Packet indicates sending bot has a negative ID. (CA_E3)\n");
|
170 |
if(recieve[2] != intersection) |
171 |
usb_puts("CA ERROR: Recieved Packet's intersection doesn't match the intersection CA is operating on in this Bot.. (CA_E4)\n");
|
172 |
#endif
|
173 |
if((*(recieve+3) & my_path) != 0){ /* Sketchy Code */ |
174 |
#ifdef DEBUG_CA
|
175 |
usb_puts("CA: My Path is occupied through intersection ");
|
176 |
usb_puti(intersection); |
177 |
usb_putc('\n');
|
178 |
#endif
|
179 |
result = false;
|
180 |
ca_sendPacket(id, 1 << first_hop | *(recieve+3)); /* Sketchy Code */ |
181 |
} |
182 |
else{
|
183 |
#ifdef DEBUG_CA
|
184 |
usb_puts("CA: My Path is clear through intersection ");
|
185 |
usb_puti(intersection); |
186 |
usb_putc('\n');
|
187 |
#endif
|
188 |
result = true;
|
189 |
ca_sendPacket(id, my_path | *(recieve+3)); /* Sketchy Code */ |
190 |
} |
191 |
return result;
|
192 |
} |