Project

General

Profile

Revision aa5e4ddc

IDaa5e4ddcdd38a8388d7157691894c250e3cb439d

Added by Leon over 6 years ago

Finishing BFS for behaviors navigationMap. Also commiting teleop skeleton code.

View differences:

scout/libscout/src/behaviors/navigationMap.cpp
14 14
   *     ---         ---        ---       ---
15 15
   */
16 16
  
17
    Edge* a1 = {MAKE_EDGE(ISTRAIGHT, 2, 10), 
18
                MAKE_EDGE(IRIGHT, 9, 40),
19
                MAKE_EDGE(IUTURN, DEADEND, 0)};
17
    Edge a1[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, 2, 10), 
18
                            MAKE_EDGE(IRIGHT, 9, 40),
19
                            MAKE_EDGE(IUTURN, DEADEND, 0)};
20 20

  
21
    Edge* a2 = {MAKE_EDGE(ISTRAIGHT, 3, 10),
22
                MAKE_EDGE(IRIGHT, 10, 40),
23
                MAKE_EDGE(IUTURN, 5, 10)};
21
    Edge a2[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, 3, 10),
22
                            MAKE_EDGE(IRIGHT, 10, 40),
23
                            MAKE_EDGE(IUTURN, 5, 10)};
24 24

  
25
    Edge* a3 = {MAKE_EDGE(ISTRAIGHT, 4, 10),
26
                MAKE_EDGE(IRIGHT, 11, 40),
27
                MAKE_EDGE(IUTURN, 6, 10)};
25
    Edge a3[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, 4, 10),
26
                            MAKE_EDGE(IRIGHT, 11, 40),
27
                            MAKE_EDGE(IUTURN, 6, 10)};
28 28

  
29
    Edge* a4 = {MAKE_EDGE(ISTRAIGHT, DEADEND, 0),
30
                MAKE_EDGE(IRIGHT, 12, 40),
31
                MAKE_EDGE(IUTURN, 7, 10)};
29
    Edge a4[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, DEADEND, 0),
30
                            MAKE_EDGE(IRIGHT, 12, 40),
31
                            MAKE_EDGE(IUTURN, 7, 10)};
32 32

  
33
    Edge* a5 = {MAKE_EDGE(ISTRAIGHT, DEADEND, 0),
34
                MAKE_EDGE(ILEFT, 9, 40),
35
                MAKE_EDGE(IUTURN, 2, 10)};
33
    Edge a5[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, DEADEND, 0),
34
                            MAKE_EDGE(ILEFT, 9, 40),
35
                            MAKE_EDGE(IUTURN, 2, 10)};
36 36

  
37
    Edge* a6 = {MAKE_EDGE(ISTRAIGHT, 5, 10),
38
                MAKE_EGE(ILEFT, 10, 40),
39
                MAKE_EDGE(IUTURN, 3, 10)};
37
    Edge a6[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, 5, 10),
38
                            MAKE_EDGE(ILEFT, 10, 40),
39
                            MAKE_EDGE(IUTURN, 3, 10)};
40 40

  
41
    Edge* a7 = {MAKE_EDGE(ISTRAIGHT, 6, 10),
42
                MAKE_EGE(ILEFT, 11, 40),
43
                MAKE_EDGE(IUTURN, 4, 10)};
41
    Edge a7[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, 6, 10),
42
                            MAKE_EDGE(ILEFT, 11, 40),
43
                            MAKE_EDGE(IUTURN, 4, 10)};
44 44

  
45
    Edge* a8 = {MAKE_EDGE(ISTRAIGHT, 7, 10),
46
                MAKE_EGE(ILEFT, 12, 40),
47
                MAKE_EDGE(IUTURN, DEADEND, 0)};
45
    Edge a8[ARRAY_SIZE] = {MAKE_EDGE(ISTRAIGHT, 7, 10),
46
                            MAKE_EDGE(ILEFT, 12, 40),
47
                            MAKE_EDGE(IUTURN, DEADEND, 0)};
48 48

  
49
    Edge* a9 = {MAKE_EDGE(IRIGHT, 2, 10),
50
                MAKE_EGE(ILEFT, DEADEND, 0),
51
                MAKE_EDGE(IUTURN, 9, 40)};
49
    Edge a9[ARRAY_SIZE] = {MAKE_EDGE(IRIGHT, 2, 10),
50
                            MAKE_EDGE(ILEFT, DEADEND, 0),
51
                            MAKE_EDGE(IUTURN, 9, 40)};
52 52

  
53
    Edge* a10 = {MAKE_EDGE(IRIGHT, 3, 10),
54
                MAKE_EGE(ILEFT, 5, 10),
55
                MAKE_EDGE(IUTURN, 10, 40)};
53
    Edge a10[ARRAY_SIZE] = {MAKE_EDGE(IRIGHT, 3, 10),
54
                            MAKE_EDGE(ILEFT, 5, 10),
55
                            MAKE_EDGE(IUTURN, 10, 40)};
56 56

  
57
    Edge* a11 = {MAKE_EDGE(IRIGHT, 4, 10),
58
                MAKE_EGE(ILEFT, 6, 10),
59
                MAKE_EDGE(IUTURN, 11, 40)};
57
    Edge a11[ARRAY_SIZE] = {MAKE_EDGE(IRIGHT, 4, 10),
58
                            MAKE_EDGE(ILEFT, 6, 10),
59
                            MAKE_EDGE(IUTURN, 11, 40)};
60 60

  
61
    Edge* a12 = {MAKE_EDGE(IRIGHT, DEADEND, 0),
62
                MAKE_EGE(ILEFT, 7, 10),
63
                MAKE_EDGE(IUTURN, 12, 40)};
61
    Edge a12[ARRAY_SIZE] = {MAKE_EDGE(IRIGHT, DEADEND, 0),
62
                            MAKE_EDGE(ILEFT, 7, 10),
63
                            MAKE_EDGE(IUTURN, 12, 40)};
64 64
      
65 65
    map.pushback(a1);
66 66
    map.pushback(a2);
......
82 82
{
83 83
    while(!map.empty())
84 84
    {
85
      Edge** e = pop_back();
86
      for(int i=0; i<ARRAY_SIZE; i++)
87
      {
88
        delete e[i];
89
      }
85
      Edge* e = map.pop_back();
86
      delete e;
90 87
    }
91 88
    return;
92 89
}
......
106 103
    // BFS algorithm
107 104
    State curr_state = get_state();
108 105
    char visited[MAX_NODES+1]; 
109
    //FAKE QUEUE OH NO 
110
    //TODO: real implementation
111
    Queue queue;
112
    queue.enqueue(curr_state);
106
    queue<State> q;
107
    q.push(curr_state);
108
    // not zero = visited, zero = unvisited, negative = start state
109
    visited[curr_state] = -1;
113 110

  
114
    // 1 == visited, 0 == not visited
115
    visited[curr_state] = 1;
116

  
117
    while (!queue.is_empty)
111
    while (!q.is_empty)
118 112
    {
119
        State state = queue.dequeue();
113
        State state = q.pop();
120 114
        if (state == target_state)
121 115
        {   
122
            //TODO: implement moves_list
123
            State i;
124

  
125
            while(i != curr_state)
116
            Turn moves_list[MAX_NODES];
117
            int j = 0; // counter
118
            for(State child = state; visited[child] >= 0; 
119
                    child = visited[child]) //while not start state
126 120
            {
127
                i = visited[i];
128

  
121
                State parent = visited[child];
122
                Edge* edges = get_outbound_edges(parent);
123
                for (int i = 0; i < ARRAY_SIZE; i++)
124
                {
125
                    if (GET_EDGE_STATE(edges[i]) == child)
126
                    {
127
                        moves_list[j] = GET_EDGE_DIR(edges[i]);
128
                        j++;
129
                        break;
130
                    }
131
                }
129 132
            }
130 133
            return moves_list;
131 134
        }
......
136 139
            State new_state = GET_EDGE_STATE(edges[i]);
137 140
            if (!visited[new_state]) 
138 141
            {
142
                // set visited to the parent of the new state
139 143
                visited[new_state] = state;
140
                queue.enqueue(new_state);
144
                q.push(new_state);
141 145
            }
142 146
        }
143 147
    }
144
}
145

  
146
navigationMap::make_edge(Turn dir, State state, int dist)
147
{
148
    Edge* e = new Edge;
149
    e->direction = dir;
150
    e->state = state;
151
    e->distance = dist;
152
    return e;
148
    //oops, no way to get to target from state
149
    return NULL;
153 150
}

Also available in: Unified diff