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