Revision 1895
More bug fixes for collision_avoid
| trunk/code/projects/traffic_navigation/collision_avoid.h | ||
|---|---|---|
| 26 | 26 |
#define D0X 1<<4 |
| 27 | 27 |
#define D1E 1<<1 |
| 28 | 28 |
#define D1X 1<<5 |
| 29 |
#define D2E 1<<6
|
|
| 30 |
#define D2X 1<<2
|
|
| 29 |
#define D2E 1<<2
|
|
| 30 |
#define D2X 1<<6
|
|
| 31 | 31 |
#define D3E 1<<3 |
| 32 | 32 |
#define D3X 1<<7 |
| 33 | 33 |
#define I0 1<<8 |
| trunk/code/projects/traffic_navigation/collision_avoid.c | ||
|---|---|---|
| 17 | 17 |
|
| 18 | 18 |
/* Dir0 |
| 19 | 19 |
* [0 ][4 ] |
| 20 |
*Dir1 [7][8 ][9 ][1] Dir3 |
|
| 21 |
* [3][11][10][5] |
|
| 22 |
* [6 ][2 ] |
|
| 23 |
* Dir2 |
|
| 24 |
*/ |
|
| 25 |
/* Dir0 |
|
| 26 |
* [0 ][4 ] |
|
| 27 | 20 |
*Dir1 [5][8 ][9 ][3] Dir3 |
| 28 | 21 |
* [1][11][10][7] |
| 29 | 22 |
* [6 ][2 ] |
| 30 | 23 |
* Dir2 |
| 24 |
* |
|
| 25 |
* Dir0 |
|
| 26 |
* [D0E][D0X] |
|
| 27 |
*Dir1 [D1X][I0][I1][D3E] Dir3 |
|
| 28 |
* [D1E][12][13][D3X] |
|
| 29 |
* [D2X][D2E] |
|
| 30 |
* Dir2 |
|
| 31 | 31 |
*/ |
| 32 | 32 |
void ca_EnterIntersection(char intersectionNum, char fromDir, char nextDir){
|
| 33 | 33 |
/* Generic turn model condenses all turns into either: |
| ... | ... | |
| 69 | 69 |
for(int i = 0; i < fromDir; i++){
|
| 70 | 70 |
int new_path = 0; |
| 71 | 71 |
char temp = my_path & DAE; |
| 72 |
char shiftoff = temp & 0x1;
|
|
| 73 |
temp = temp >> 1;
|
|
| 74 |
temp = temp | (shiftoff << 3);
|
|
| 72 |
char shiftoff = temp & 0x8;
|
|
| 73 |
temp = temp << 1;
|
|
| 74 |
temp = temp | (shiftoff >> 3);
|
|
| 75 | 75 |
new_path | temp; |
| 76 | 76 |
|
| 77 | 77 |
char temp = (my_path & DAX) >> DAXS; |
| 78 | 78 |
char shiftoff = temp & 0x1; |
| 79 |
temp = temp >> 1;
|
|
| 80 |
temp = temp | (shiftoff << 3);
|
|
| 81 |
new_path | temp;
|
|
| 79 |
temp = temp << 1;
|
|
| 80 |
temp = temp | (shiftoff >> 3);
|
|
| 81 |
new_path | (temp << DAXS);
|
|
| 82 | 82 |
|
| 83 |
char temp = (my_path & IA) >> IAS;
|
|
| 83 |
char temp = (my_path & DAX) >> IAS;
|
|
| 84 | 84 |
char shiftoff = temp & 0x1; |
| 85 |
temp = temp >> 1; |
|
| 86 |
temp = temp | (shiftoff << 3); |
|
| 87 |
new_path | temp; |
|
| 85 |
temp = temp << 1; |
|
| 86 |
temp = temp | (shiftoff >> 3); |
|
| 87 |
new_path | (temp << IAS); |
|
| 88 |
mypath = new_path; |
|
| 89 |
|
|
| 88 | 90 |
} |
| 89 |
my_path = new_path; |
|
| 90 |
firsthop = |
|
| 91 |
firsthop = fromDir; |
|
| 91 | 92 |
} |
| 92 | 93 |
void ca_ExitIntersection(){
|
| 93 | 94 |
mypath = 0; |
| ... | ... | |
| 130 | 131 |
usb_putc('\n');
|
| 131 | 132 |
#endif |
| 132 | 133 |
result = false; |
| 133 |
ca_sendPacket(*(recieve+3)); /* Sketchy Code */ |
|
| 134 |
ca_sendPacket(1 << fisthop | *(recieve+3)); /* Sketchy Code */
|
|
| 134 | 135 |
} |
| 135 | 136 |
else{
|
| 136 | 137 |
#ifdef DEBUG_CA |
Also available in: Unified diff