Revision 1898
More bugfixes (holy crap I'm bad at this)
trunk/code/projects/traffic_navigation/collision_avoid.c | ||
---|---|---|
45 | 45 |
*/ |
46 | 46 |
/* Fill in dir0 */ |
47 | 47 |
my_path = 0; |
48 |
my_path | D0E; |
|
49 |
my_path | I0; |
|
48 |
my_path = my_path | D0E;
|
|
49 |
my_path = my_path | I0;
|
|
50 | 50 |
char genericNext = nextDir-fromDir; |
51 | 51 |
if(genericNext == 1){ //right turn |
52 | 52 |
#ifdef DEBUG_CA |
53 | 53 |
usb_puts("CA: Setting up Generic Right Turn\n"); |
54 | 54 |
#endif |
55 |
my_path | D1X; |
|
55 |
my_path = my_path | D1X;
|
|
56 | 56 |
} |
57 | 57 |
else if(genericNext == 2){ //straight turn |
58 | 58 |
#ifdef DEBUG_CA |
59 | 59 |
usb_puts("CA: Setting up Generic Straight Turn\n"); |
60 | 60 |
#endif |
61 |
my_path | D2X; |
|
62 |
my_path | I2; |
|
61 |
my_path = my_path | D2X;
|
|
62 |
my_path = my_path | I2;
|
|
63 | 63 |
} |
64 | 64 |
else{ //left turn |
65 | 65 |
#ifdef DEBUG_CA |
... | ... | |
67 | 67 |
usb_puts("CA ERROR: Error in generic turn generator (CA_E0)\n"); |
68 | 68 |
usb_puts("CA: Setting up Generic Left Turn\n"); |
69 | 69 |
#endif |
70 |
my_path | D3X; |
|
71 |
my_path | I2; |
|
72 |
my_path | I3; |
|
70 |
my_path = my_path | D3X;
|
|
71 |
my_path = my_path | I2;
|
|
72 |
my_path = my_path | I3;
|
|
73 | 73 |
|
74 | 74 |
} |
75 | 75 |
//now rotate turn to match what it's really supposed to be |
... | ... | |
79 | 79 |
char shiftoff = temp & 0x8; |
80 | 80 |
temp = temp << 1; |
81 | 81 |
temp = temp | (shiftoff >> 3); |
82 |
new_path | temp; |
|
82 |
new_path = new_path | temp;
|
|
83 | 83 |
|
84 |
char temp = (my_path & DAX) >> DAXS;
|
|
85 |
char shiftoff = temp & 0x1;
|
|
84 |
temp = (my_path & DAX) >> DAXS; |
|
85 |
shiftoff = temp & 0x1; |
|
86 | 86 |
temp = temp << 1; |
87 | 87 |
temp = temp | (shiftoff >> 3); |
88 |
new_path | (temp << DAXS); |
|
88 |
new_path = new_path | (temp << DAXS);
|
|
89 | 89 |
|
90 |
char temp = (my_path & DAX) >> IAS;
|
|
91 |
char shiftoff = temp & 0x1;
|
|
90 |
temp = (my_path & DAX) >> IAS; |
|
91 |
shiftoff = temp & 0x1; |
|
92 | 92 |
temp = temp << 1; |
93 | 93 |
temp = temp | (shiftoff >> 3); |
94 |
new_path | (temp << IAS); |
|
95 |
mypath = new_path; |
|
94 |
new_path = new_path | (temp << IAS);
|
|
95 |
my_path = new_path;
|
|
96 | 96 |
|
97 | 97 |
} |
98 |
firsthop = fromDir; |
|
99 |
intersection = intersectionNum |
|
98 |
first_hop = fromDir;
|
|
99 |
intersection = intersectionNum;
|
|
100 | 100 |
} |
101 | 101 |
|
102 | 102 |
/* Call this function when the bot has left the intersection. |
... | ... | |
104 | 104 |
* All this function does is clear some variables. I guess |
105 | 105 |
* this is optional but it is nice for debugging. */ |
106 | 106 |
void ca_ExitIntersection(){ |
107 |
mypath = 0; |
|
108 |
firsthop = -1; |
|
107 |
my_path = 0;
|
|
108 |
first_hop = -1;
|
|
109 | 109 |
intersection = -1; |
110 | 110 |
} |
111 | 111 |
|
... | ... | |
170 | 170 |
if(recieve[2] != intersection) |
171 | 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 | 172 |
#endif |
173 |
if(*(recieve+3) & my_path != 0){ /* Sketchy Code */
|
|
173 |
if((*(recieve+3) & my_path) != 0){ /* Sketchy Code */
|
|
174 | 174 |
#ifdef DEBUG_CA |
175 | 175 |
usb_puts("CA: My Path is occupied through intersection "); |
176 | 176 |
usb_puti(intersection); |
177 | 177 |
usb_putc('\n'); |
178 | 178 |
#endif |
179 | 179 |
result = false; |
180 |
ca_sendPacket(id, 1 << fisthop | *(recieve+3)); /* Sketchy Code */
|
|
180 |
ca_sendPacket(id, 1 << first_hop | *(recieve+3)); /* Sketchy Code */
|
|
181 | 181 |
} |
182 | 182 |
else{ |
183 | 183 |
#ifdef DEBUG_CA |
Also available in: Unified diff