Revision 1886
Tested and finished the database and the valid turns and everything!
database:
files: intersectData.c and intersectData.h
database is an array defined in .h that stores all intersections
right now it is defined for our demo map.
it has an intialize function that is unique for each map and all
other functions that can be reused. (constracted in this way
so that we do not need to keep rewriting code)
validTurns:
integrated with the database. uses database lookup functions to
determine intersection type and position from the given barcode
and determines whether the given turn is valid.
Yay for being done!!
| trunk/code/projects/traffic_navigation/validTurns.h | ||
|---|---|---|
| 12 | 12 |
/******************************** |
| 13 | 13 |
* Intersection TYPES: |
| 14 | 14 |
* |
| 15 |
* INTERSECTION_SINGLE (0 << 2) |
|
| 16 |
* INTERSECTION_ON_RAMP (1 << 2) |
|
| 17 |
* INTERSECTION_OFF_RAMP (2 << 2) |
|
| 18 |
* INTERSECTION_DOUBLE_C (3 << 2) |
|
| 19 |
* INTERSECTION_DOUBLE_T (4 << 2) |
|
| 20 |
* |
|
| 15 | 21 |
********************************/ |
| 16 | 22 |
|
| 23 |
/******************************** |
|
| 24 |
* Turn TYPES: |
|
| 25 |
* |
|
| 26 |
* ISTRAIGHT 0 |
|
| 27 |
* ILEFT 1 |
|
| 28 |
* IRIGHT 2 |
|
| 29 |
* IUTURN 3 |
|
| 30 |
* |
|
| 31 |
********************************/ |
|
| 17 | 32 |
|
| 18 |
#define INTERSECTION_SINGLE (0 << 2) |
|
| 19 |
#define INTERSECTION_ON_RAMP (1 << 2) |
|
| 20 |
#define INTERSECTION_OFF_RAMP (2 << 2) |
|
| 21 |
#define INTERSECTION_DOUBLE_C (3 << 2) |
|
| 22 |
#define INTERSECTION_DOUBLE_T (4 << 2) |
|
| 23 | 33 |
|
| 24 |
|
|
| 25 | 34 |
/******************************** |
| 26 | 35 |
* Intersection ENTRYPOINTS: |
| 36 |
* INTERSECTION_DOUBLE_C |
|
| 37 |
* use T-Junction positions |
|
| 38 |
* |
|
| 39 |
* INTERSECTION_DOUBLE_T |
|
| 40 |
* TLEFT 0 |
|
| 41 |
* TRIGHT 1 |
|
| 42 |
* TMIDDLE 2 |
|
| 43 |
* |
|
| 44 |
* INTERSECTION_SINGLE |
|
| 45 |
* SACROSS 0 |
|
| 46 |
* SUP 1 |
|
| 47 |
* |
|
| 48 |
* INTERSECTION_ON_RAMP |
|
| 49 |
* R_LEFT 0 |
|
| 50 |
* R_RIGHT 1 |
|
| 51 |
* R_RAMP 2 |
|
| 52 |
* |
|
| 53 |
* INTERSECTION_OFF_RAMP |
|
| 54 |
* use On_ramp positions |
|
| 27 | 55 |
* |
| 28 |
********************************/ |
|
| 29 |
|
|
| 30 |
//DOUBLE intersection (Cross Intersection) positions |
|
| 31 |
//use T-Junction positions |
|
| 32 |
|
|
| 33 |
//DOUBLE intersection (T-Junction) positions |
|
| 34 |
/* |
|
| 35 |
TLEFT=======TRIGHT |
|
| 36 |
|| |
|
| 37 |
|| |
|
| 38 |
|| |
|
| 39 |
TMIDDLE |
|
| 40 |
*/ |
|
| 41 |
#define TLEFT 0 |
|
| 42 |
#define TRIGHT 1 |
|
| 43 |
#define TMIDDLE 2 |
|
| 44 |
|
|
| 45 |
//SINGLE intersection positions |
|
| 46 |
/* |
|
| 47 |
/\ |
|
| 48 |
/||\ |
|
| 49 |
|| |
|
| 50 |
SACROSS==========> |
|
| 51 |
|| |
|
| 52 |
|| |
|
| 53 |
|| |
|
| 54 |
SUP |
|
| 55 |
*/ |
|
| 56 |
#define SACROSS 0 |
|
| 57 |
#define SUP 1 |
|
| 58 |
|
|
| 59 |
|
|
| 60 |
//ON_RAMP and OFF_RAMP intersection positions |
|
| 61 |
/* |
|
| 62 |
R_LEFT================R_RIGHT |
|
| 63 |
|| |
|
| 64 |
|| |
|
| 65 |
|| |
|
| 66 |
R_RAMP |
|
| 67 |
*/ |
|
| 68 |
#define R_LEFT 0 |
|
| 69 |
#define R_RIGHT 1 |
|
| 70 |
#define R_RAMP 2 |
|
| 71 |
|
|
| 56 |
*****************************/ |
|
| 57 |
|
|
| 72 | 58 |
int validateTurn(int barcode, int turn_type); |
| 73 | 59 |
|
| 74 | 60 |
#endif |
| trunk/code/projects/traffic_navigation/validTurns-test.c | ||
|---|---|---|
| 1 | 1 |
#include <dragonfly_lib.h> |
| 2 |
#include "lineDrive.h"
|
|
| 2 |
#include "intersectData.h"
|
|
| 3 | 3 |
#include "validTurns.h" |
| 4 | 4 |
|
| 5 |
#define TEST_VALIDTURNS |
|
| 6 |
|
|
| 5 | 7 |
#ifdef TEST_VALIDTURNS |
| 6 | 8 |
|
| 7 | 9 |
int main(){
|
| 8 |
/*barcode should be written as crosstype, crosspos. 5 bit integers*/ |
|
| 9 | 10 |
|
| 10 |
/*barcode should be three bits of intersection type and 2 of entrypoint*/ |
|
| 11 |
dragonfly_init(ALL_ON); |
|
| 12 |
orb1_set_color(YELLOW); |
|
| 11 | 13 |
|
| 12 |
dragonfly_init(ALL_ON); |
|
| 13 |
orb1_set_color(YELLOW); |
|
| 14 |
initializeData(); |
|
| 15 |
|
|
| 16 |
int barcode = 0; |
|
| 17 |
|
|
| 18 |
if (validateTurn(barcode, ILEFT)==ILEFT) orb1_set_color(GREEN); |
|
| 14 | 19 |
|
| 15 |
int barcode1 = (DOUBLE_C<<2)+(TMIDDLE); |
|
| 16 |
if (validateTurn(barcode1, ILEFT)==ILEFT) orb1_set_color(GREEN); |
|
| 20 |
delay_ms(100); |
|
| 21 |
orb1_set_color(PINK); |
|
| 22 |
delay_ms(50); |
|
| 17 | 23 |
|
| 18 |
delay_ms(100); |
|
| 19 |
orb1_set_color(PINK); |
|
| 20 |
delay_ms(50); |
|
| 24 |
if (validateTurn(barcode, IUTURN)==IUTURN) orb1_set_color(GREEN); |
|
| 25 |
|
|
| 26 |
delay_ms(100); |
|
| 27 |
orb1_set_color(RED); |
|
| 28 |
delay_ms(50); |
|
| 21 | 29 |
|
| 22 |
int barcode2 = (DOUBLE_C<<2)+(TMIDDLE); |
|
| 23 |
if (validateTurn(barcode2, IUTURN)==IUTURN) orb1_set_color(GREEN); |
|
| 30 |
while (1){}
|
|
| 24 | 31 |
|
| 25 |
delay_ms(100); |
|
| 26 |
orb1_set_color(RED); |
|
| 27 |
delay_ms(50); |
|
| 28 |
|
|
| 29 |
int barcode3 = (DOUBLE_T<<2)+(TMIDDLE); |
|
| 30 |
if (validateTurn(barcode3, IUTURN)==IUTURN) orb1_set_color(GREEN); |
|
| 31 |
|
|
| 32 |
delay_ms(100); |
|
| 33 |
orb1_set_color(PURPLE); |
|
| 34 |
delay_ms(50); |
|
| 35 |
|
|
| 36 |
int barcode4 = (DOUBLE_T<<2)+(TMIDDLE); |
|
| 37 |
if (validateTurn(barcode4, ISTRAIGHT)==IUTURN) orb1_set_color(GREEN); |
|
| 38 |
|
|
| 39 |
while (1){}
|
|
| 40 |
|
|
| 41 | 32 |
} |
| 42 | 33 |
|
| 43 |
#endif TEST_VALIDTURNS |
|
| 34 |
#endif |
|
| trunk/code/projects/traffic_navigation/validTurns.c | ||
|---|---|---|
| 7 | 7 |
* |
| 8 | 8 |
*/ |
| 9 | 9 |
|
| 10 |
#include <dragonfly_lib.h> |
|
| 11 |
#include "validTurns.h" |
|
| 12 | 10 |
|
| 13 |
|
|
| 14 | 11 |
/******************************Random Num Gen Version of validTurns |
| 15 | 12 |
int randomNumGen(int max){
|
| 16 | 13 |
int x = range_read_distance(IR2); |
| ... | ... | |
| 18 | 15 |
else return randomNumGen(max); |
| 19 | 16 |
} |
| 20 | 17 |
|
| 21 |
int getCrossType(int barcode)
|
|
| 18 |
int getIntersectType(int barcode)
|
|
| 22 | 19 |
{
|
| 23 | 20 |
int x = randomNumGen(4); |
| 24 | 21 |
if (x == DOUBLE) {
|
| ... | ... | |
| 29 | 26 |
return x; |
| 30 | 27 |
} |
| 31 | 28 |
|
| 32 |
int getCrossPos(int barcode, int max)
|
|
| 29 |
int getIntersectPos(int barcode, int max)
|
|
| 33 | 30 |
{
|
| 34 | 31 |
return randomNumGen(max); |
| 35 | 32 |
} |
| ... | ... | |
| 39 | 36 |
return randomNumGen(4); |
| 40 | 37 |
} |
| 41 | 38 |
|
| 42 |
int getCrossType(int barcode){
|
|
| 43 |
int crosstype = (barcode>>2)&7; |
|
| 44 |
return crosstype; |
|
| 45 |
} |
|
| 46 |
int getCrossPos(int barcode){
|
|
| 47 |
return (barcode)&3; |
|
| 48 |
} |
|
| 49 |
|
|
| 50 | 39 |
*/ |
| 51 | 40 |
|
| 41 |
#include "intersectData.h" |
|
| 42 |
#include "validTurns.h" |
|
| 43 |
|
|
| 52 | 44 |
int validateTurn(int barcode, int turn_type) |
| 53 | 45 |
{
|
| 54 |
int cross_type;
|
|
| 55 |
int cross_pos;
|
|
| 56 |
cross_type = getCrossType(barcode);
|
|
| 57 |
switch (cross_type)
|
|
| 46 |
int intersect_type;
|
|
| 47 |
int intersect_pos;
|
|
| 48 |
intersect_type = getIntersectType(barcode);
|
|
| 49 |
switch (intersect_type)
|
|
| 58 | 50 |
{
|
| 59 | 51 |
case INTERSECTION_DOUBLE_C: |
| 60 | 52 |
{
|
| 61 |
cross_pos = getCrossPos(barcode, 4);
|
|
| 62 |
if (0<=cross_pos && cross_pos<=3)
|
|
| 53 |
intersect_pos = getIntersectPos(barcode);
|
|
| 54 |
if (0<=intersect_pos && intersect_pos<=3)
|
|
| 63 | 55 |
return turn_type; |
| 64 | 56 |
break; |
| 65 | 57 |
} |
| 66 | 58 |
case INTERSECTION_DOUBLE_T: |
| 67 | 59 |
{
|
| 68 |
cross_pos = getCrossPos(barcode, 3);
|
|
| 69 |
switch (cross_pos)
|
|
| 60 |
intersect_pos = getIntersectPos(barcode);
|
|
| 61 |
switch (intersect_pos)
|
|
| 70 | 62 |
{
|
| 71 | 63 |
case TLEFT: |
| 72 | 64 |
{
|
| ... | ... | |
| 93 | 85 |
} |
| 94 | 86 |
case INTERSECTION_SINGLE: |
| 95 | 87 |
{
|
| 96 |
cross_pos = getCrossPos(barcode, 2);
|
|
| 97 |
switch (cross_pos)
|
|
| 88 |
intersect_pos = getIntersectPos(barcode);
|
|
| 89 |
switch (intersect_pos)
|
|
| 98 | 90 |
{
|
| 99 | 91 |
case SACROSS: |
| 100 | 92 |
{
|
| ... | ... | |
| 115 | 107 |
} |
| 116 | 108 |
case INTERSECTION_ON_RAMP: |
| 117 | 109 |
{
|
| 118 |
cross_pos = getCrossPos(barcode, 3);
|
|
| 119 |
switch (cross_pos)
|
|
| 110 |
intersect_pos = getIntersectPos(barcode);
|
|
| 111 |
switch (intersect_pos)
|
|
| 120 | 112 |
{
|
| 121 | 113 |
case R_LEFT: |
| 122 | 114 |
{
|
| ... | ... | |
| 137 | 129 |
} |
| 138 | 130 |
case INTERSECTION_OFF_RAMP: |
| 139 | 131 |
{
|
| 140 |
cross_pos = getCrossPos(barcode, 3);
|
|
| 141 |
switch (cross_pos)
|
|
| 132 |
intersect_pos = getIntersectPos(barcode);
|
|
| 133 |
switch (intersect_pos)
|
|
| 142 | 134 |
{
|
| 143 | 135 |
case R_LEFT: |
| 144 | 136 |
{
|
Also available in: Unified diff