Revision d8caf546
ID | d8caf54695b0f88d1d2eb6afb9e47d8dfcdfba47 |
Moved outboundVectTable to navigationMap and filled out the code for it.
scout/libscout/src/behaviors/lineDrive.h | ||
---|---|---|
1 | 1 |
#ifndef _LINE_DRIVE_ |
2 | 2 |
#define _LINE_DRIVE_ |
3 | 3 |
|
4 |
#include "../Behavior.h" |
|
4 | 5 |
#include "lineFollow.h" |
5 | 6 |
|
6 | 7 |
/* |
scout/libscout/src/behaviors/navigationMap.cpp | ||
---|---|---|
1 |
#include "navigationMap.h" |
|
2 |
|
|
3 |
/** @brief Initializes the map */ |
|
4 |
navigationMap::navigationMap(std::string scoutname) : Behavior(scoutname) |
|
5 |
{ |
|
6 |
/** Initialize Map |
|
7 |
* |
|
8 |
* 1 2 3 4 |
|
9 |
* ----|-----------|----------|---------|----------> |
|
10 |
* <---|--5--------|--6-------|--7------|--8------- |
|
11 |
* | | | | |
|
12 |
* 9| 10| 11| 12| |
|
13 |
* | | | | |
|
14 |
* --- --- --- --- |
|
15 |
*/ |
|
16 |
|
|
17 |
Edge* a1 = {MAKE_EDGE(ISTRAIGHT, 2, 10), |
|
18 |
MAKE_EDGE(IRIGHT, 9, 40), |
|
19 |
MAKE_EDGE(IUTURN, DEADEND, 0)}; |
|
20 |
|
|
21 |
Edge* a2 = {MAKE_EDGE(ISTRAIGHT, 3, 10), |
|
22 |
MAKE_EDGE(IRIGHT, 10, 40), |
|
23 |
MAKE_EDGE(IUTURN, 5, 10)}; |
|
24 |
|
|
25 |
Edge* a3 = {MAKE_EDGE(ISTRAIGHT, 4, 10), |
|
26 |
MAKE_EDGE(IRIGHT, 11, 40), |
|
27 |
MAKE_EDGE(IUTURN, 6, 10)}; |
|
28 |
|
|
29 |
Edge* a4 = {MAKE_EDGE(ISTRAIGHT, DEADEND, 0), |
|
30 |
MAKE_EDGE(IRIGHT, 12, 40), |
|
31 |
MAKE_EDGE(IUTURN, 7, 10)}; |
|
32 |
|
|
33 |
Edge* a5 = {MAKE_EDGE(ISTRAIGHT, DEADEND, 0), |
|
34 |
MAKE_EDGE(ILEFT, 9, 40), |
|
35 |
MAKE_EDGE(IUTURN, 2, 10)}; |
|
36 |
|
|
37 |
Edge* a6 = {MAKE_EDGE(ISTRAIGHT, 5, 10), |
|
38 |
MAKE_EGE(ILEFT, 10, 40), |
|
39 |
MAKE_EDGE(IUTURN, 3, 10)}; |
|
40 |
|
|
41 |
Edge* a7 = {MAKE_EDGE(ISTRAIGHT, 6, 10), |
|
42 |
MAKE_EGE(ILEFT, 11, 40), |
|
43 |
MAKE_EDGE(IUTURN, 4, 10)}; |
|
44 |
|
|
45 |
Edge* a8 = {MAKE_EDGE(ISTRAIGHT, 7, 10), |
|
46 |
MAKE_EGE(ILEFT, 12, 40), |
|
47 |
MAKE_EDGE(IUTURN, DEADEND, 0)}; |
|
48 |
|
|
49 |
Edge* a9 = {MAKE_EDGE(IRIGHT, 2, 10), |
|
50 |
MAKE_EGE(ILEFT, DEADEND, 0), |
|
51 |
MAKE_EDGE(IUTURN, 9, 40)}; |
|
52 |
|
|
53 |
Edge* a10 = {MAKE_EDGE(IRIGHT, 3, 10), |
|
54 |
MAKE_EGE(ILEFT, 5, 10), |
|
55 |
MAKE_EDGE(IUTURN, 10, 40)}; |
|
56 |
|
|
57 |
Edge* a11 = {MAKE_EDGE(IRIGHT, 4, 10), |
|
58 |
MAKE_EGE(ILEFT, 6, 10), |
|
59 |
MAKE_EDGE(IUTURN, 11, 40)}; |
|
60 |
|
|
61 |
Edge* a12 = {MAKE_EDGE(IRIGHT, DEADEND, 0), |
|
62 |
MAKE_EGE(ILEFT, 7, 10), |
|
63 |
MAKE_EDGE(IUTURN, 12, 40)}; |
|
64 |
|
|
65 |
map.pushback(a1); |
|
66 |
map.pushback(a2); |
|
67 |
map.pushback(a3); |
|
68 |
map.pushback(a4); |
|
69 |
map.pushback(a5); |
|
70 |
map.pushback(a6); |
|
71 |
map.pushback(a7); |
|
72 |
map.pushback(a8); |
|
73 |
map.pushback(a9); |
|
74 |
map.pushback(a10); |
|
75 |
map.pushback(a11); |
|
76 |
map.pushback(a12); |
|
77 |
|
|
78 |
} |
|
79 |
|
|
80 |
/** @brief Goes through and frees all allocated memory */ |
|
81 |
navigationMap::~navigationMap() |
|
82 |
{ |
|
83 |
while(!map.empty()) |
|
84 |
{ |
|
85 |
Edge** e = pop_back(); |
|
86 |
for(int i=0; i<ARRAY_SIZE; i++) |
|
87 |
{ |
|
88 |
delete e[i]; |
|
89 |
} |
|
90 |
} |
|
91 |
return; |
|
92 |
} |
|
93 |
|
|
94 |
navigationMap::run() |
|
95 |
{ |
|
96 |
|
|
97 |
} |
|
98 |
|
|
99 |
navigationMap::get_state() |
|
100 |
{ |
|
101 |
return curr_state; |
|
102 |
} |
|
103 |
|
|
104 |
navigationMap::shortest_path(State target_state) |
|
105 |
{ |
|
106 |
// BFS algorithm |
|
107 |
} |
|
108 |
|
|
109 |
navigationMap::make_edge(Turn dir, State state, int dist) |
|
110 |
{ |
|
111 |
Edge* e = new Edge; |
|
112 |
e->direction = dir; |
|
113 |
e->state = state; |
|
114 |
e->distance = dist; |
|
115 |
return e; |
|
116 |
} |
scout/libscout/src/behaviors/navigationMap.h | ||
---|---|---|
1 |
#ifndef _NAVIGATION_MAP_ |
|
2 |
#define _NAVIGATION_MAP_ |
|
3 |
|
|
4 |
#include <cstdlib> |
|
5 |
#include "linieDrive.h" // Get turn Macros |
|
6 |
|
|
7 |
#define START_STATE 1 |
|
8 |
|
|
9 |
#define DEADEND -1 |
|
10 |
#define ARRAY_SIZE 3 |
|
11 |
|
|
12 |
#define GET_EDGE_DIR(edge) ((edge)&0x3) |
|
13 |
#define GET_EDGE_STATE(edge) (((edge)>>2)&0xFF) |
|
14 |
#define GET_EDGE_DIST(edge) (((edge)>>10)&0x3FFFFF) |
|
15 |
|
|
16 |
#define SET_EDGE_DIR(dir) ((dir)&0x3) |
|
17 |
#define SET_EDGE_STATE(state) (((state)&0xFF)<<2) |
|
18 |
#define SET_EDGE_DIST(dist) (((dist)&0x3FFFFF)<<10) |
|
19 |
|
|
20 |
#define MAKE_EDGE(dir, state, dist) \ |
|
21 |
SET_EDGE_DIR(dir)+SET_EDGE_STATE(state)+SET_EDGE_DIST(dist) |
|
22 |
|
|
23 |
typedef int Edge; |
|
24 |
typedef int State; |
|
25 |
typedef int Turn; |
|
26 |
|
|
27 |
class navigationMap : Behavior |
|
28 |
{ |
|
29 |
public: |
|
30 |
navigationMap(std::string scoutname); |
|
31 |
~navigationMap(); |
|
32 |
|
|
33 |
void run(); |
|
34 |
|
|
35 |
State get_state(); |
|
36 |
Turn* shortest_path(State target_state); |
|
37 |
|
|
38 |
Edge* get_outbound_edges(); |
|
39 |
|
|
40 |
private: |
|
41 |
vector <Edge*> map; |
|
42 |
State curr_state; |
|
43 |
|
|
44 |
Edge* make_edge(Turn dir, State state, int dist); |
|
45 |
|
|
46 |
}; |
|
47 |
#endif |
scout/libscout/src/behaviors/outboundVectTable.cpp | ||
---|---|---|
1 |
#include "navigationMap.h" |
|
2 |
|
|
3 |
void navigationMap(std::string scoutname) : Behavior(scoutname) |
|
4 |
{ |
|
5 |
|
|
6 |
} |
scout/libscout/src/behaviors/outboundVectTable.h | ||
---|---|---|
1 |
#ifndef _NAVIGATION_MAP_ |
|
2 |
#define _NAVIGATION_MAP_ |
|
3 |
|
|
4 |
#include <cstdlib> |
|
5 |
|
|
6 |
struct edge |
|
7 |
{ |
|
8 |
int direction; /// direction to turn |
|
9 |
int index; /// map index |
|
10 |
int distance; /// physical distance |
|
11 |
}; |
|
12 |
typedef struct edge edge; |
|
13 |
|
|
14 |
class navigationMap : Behavior |
|
15 |
{ |
|
16 |
public: |
|
17 |
navigationMap(std::string scoutname); |
|
18 |
|
|
19 |
void run(); |
|
20 |
|
|
21 |
int get_state(); |
|
22 |
int* shortest_path(int target_state); |
|
23 |
|
|
24 |
edge* get_outbound_edges(); |
|
25 |
|
|
26 |
private: |
|
27 |
edge** map; |
|
28 |
}; |
|
29 |
#endif |
Also available in: Unified diff