root / trunk / code / projects / traffic_navigation / collision_avoid.c @ 1902
History | View | Annotate | Download (6.07 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 | 1894 | azl | */
|
7 | 1892 | azl | #include "collision_avoid.h" |
8 | #include "traffic_navigation.h" |
||
9 | static unsigned int my_path = 0; |
||
10 | 1894 | azl | static unsigned int first_hop = 0; |
11 | 1892 | azl | static char intersection = 0; |
12 | |||
13 | /* Dir0
|
||
14 | * [0 ][4 ]
|
||
15 | 1894 | azl | *Dir1 [5][8 ][9 ][3] Dir3
|
16 | * [1][11][10][7]
|
||
17 | * [6 ][2 ]
|
||
18 | * Dir2
|
||
19 | 1895 | azl | *
|
20 | * Dir0
|
||
21 | * [D0E][D0X]
|
||
22 | *Dir1 [D1X][I0][I1][D3E] Dir3
|
||
23 | * [D1E][12][13][D3X]
|
||
24 | * [D2X][D2E]
|
||
25 | * Dir2
|
||
26 | 1894 | azl | */
|
27 | 1897 | azl | |
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 | 1892 | azl | 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 | 1898 | azl | my_path = my_path | D0E; |
49 | my_path = my_path | I0; |
||
50 | 1892 | azl | 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 | 1898 | azl | my_path = my_path | D1X; |
56 | 1892 | azl | } |
57 | else if(genericNext == 2){ //straight turn |
||
58 | #ifdef DEBUG_CA
|
||
59 | usb_puts("CA: Setting up Generic Straight Turn\n");
|
||
60 | #endif
|
||
61 | 1898 | azl | my_path = my_path | D2X; |
62 | my_path = my_path | I2; |
||
63 | 1892 | azl | } |
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 | 1898 | azl | my_path = my_path | D3X; |
71 | my_path = my_path | I2; |
||
72 | my_path = my_path | I3; |
||
73 | 1892 | azl | |
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 | 1895 | azl | char shiftoff = temp & 0x8; |
80 | temp = temp << 1;
|
||
81 | temp = temp | (shiftoff >> 3);
|
||
82 | 1898 | azl | new_path = new_path | temp; |
83 | 1892 | azl | |
84 | 1898 | azl | temp = (my_path & DAX) >> DAXS; |
85 | shiftoff = temp & 0x1;
|
||
86 | 1895 | azl | temp = temp << 1;
|
87 | temp = temp | (shiftoff >> 3);
|
||
88 | 1898 | azl | new_path = new_path | (temp << DAXS); |
89 | 1892 | azl | |
90 | 1898 | azl | temp = (my_path & DAX) >> IAS; |
91 | shiftoff = temp & 0x1;
|
||
92 | 1895 | azl | temp = temp << 1;
|
93 | temp = temp | (shiftoff >> 3);
|
||
94 | 1898 | azl | new_path = new_path | (temp << IAS); |
95 | my_path = new_path; |
||
96 | 1895 | azl | |
97 | 1892 | azl | } |
98 | 1898 | azl | first_hop = fromDir; |
99 | intersection = intersectionNum; |
||
100 | 1892 | azl | } |
101 | 1897 | azl | |
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 | 1892 | azl | void ca_ExitIntersection(){
|
107 | 1898 | azl | my_path = 0;
|
108 | first_hop = -1;
|
||
109 | 1897 | azl | intersection = -1;
|
110 | 1892 | azl | } |
111 | 1897 | azl | |
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 | 1892 | azl | 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 | 1897 | azl | |
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 | 1892 | azl | 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 | 1897 | azl | ca_sendPacket(id, my_path); |
145 | 1892 | azl | } |
146 | 1897 | azl | |
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 | 1892 | azl | 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 | 1894 | azl | 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 | 1892 | azl | #endif
|
173 | 1898 | azl | if((*(recieve+3) & my_path) != 0){ /* Sketchy Code */ |
174 | 1892 | azl | #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 | 1898 | azl | ca_sendPacket(id, 1 << first_hop | *(recieve+3)); /* Sketchy Code */ |
181 | 1892 | azl | } |
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 | 1897 | azl | ca_sendPacket(id, my_path | *(recieve+3)); /* Sketchy Code */ |
190 | 1892 | azl | } |
191 | return result;
|
||
192 | } |