root / branches / lemmings / code / behaviors / lemmings_ibrin / lemmings.c @ 332
History | View | Annotate | Download (4.76 KB)
1 | 212 | dsschult | #include <dragonfly_lib.h> |
---|---|---|---|
2 | #include <wl_defs.h> |
||
3 | #include <wireless.h> |
||
4 | #include <wl_token_ring.h> |
||
5 | #include "smart_run_around_fsm.h" |
||
6 | #include "lemmings.h" |
||
7 | |||
8 | /*A simple behavior for following the leader.
|
||
9 | |||
10 | SINGLE_FILE pattern not implemented yet
|
||
11 | */
|
||
12 | |||
13 | |||
14 | void lemmings_init()
|
||
15 | { |
||
16 | dragonfly_init(ALL_ON); |
||
17 | wl_init(); |
||
18 | run_around_init(); // prepare for moving
|
||
19 | /*
|
||
20 | analog_init();
|
||
21 | motors_init();
|
||
22 | usb_init();
|
||
23 | usb_puts("initted usb\n");
|
||
24 | wl_init();
|
||
25 | usb_puts("initted wireless\n");
|
||
26 | */
|
||
27 | wl_token_ring_register(); |
||
28 | usb_puts("registered for token ring\n");
|
||
29 | wl_token_ring_join(); // join token ring
|
||
30 | usb_puts("joined token ring\n");
|
||
31 | /*Start in the default state, LEAD*/
|
||
32 | cur_state = LEAD; |
||
33 | /*Set timers to their maximum values.*/
|
||
34 | crazy_count=CRAZY_MAX; |
||
35 | |||
36 | // set to default state
|
||
37 | follow_bot = -1;
|
||
38 | cur_state = LEAD; |
||
39 | |||
40 | // set pattern
|
||
41 | lemmings_set_pattern(FOLLOW_MULTI_DEFAULT); |
||
42 | usb_puts("initted successfully\n");
|
||
43 | } |
||
44 | |||
45 | // set the following pattern
|
||
46 | // SINGLE_FILE if pattern <= 0
|
||
47 | // TREE if pattern >= 1
|
||
48 | void lemmings_set_pattern(int pattern) |
||
49 | { |
||
50 | if (pattern <= 0) |
||
51 | follow_multi = 0;
|
||
52 | else
|
||
53 | follow_multi = 1;
|
||
54 | } |
||
55 | |||
56 | /*The main function, call this to update states as frequently as possible.*/
|
||
57 | void lemmings_FSM(void) { |
||
58 | int bom=0; |
||
59 | |||
60 | if (follow_bot < 0) |
||
61 | lemmings_Prims(); // if we are not following, then attempt to follow
|
||
62 | |||
63 | usb_puts("following: ");
|
||
64 | usb_puti(follow_bot); |
||
65 | |||
66 | // if we are following a bot, make sure we can still see it
|
||
67 | while (follow_bot >= 0) { |
||
68 | bom = wl_token_get_sensor_reading(wl_get_xbee_id(),follow_bot); |
||
69 | if (bom < 0) { |
||
70 | // get new bot to follow
|
||
71 | lemmings_Prims(); |
||
72 | continue;
|
||
73 | } |
||
74 | else {
|
||
75 | cur_state = FOLLOW; // set to follow state
|
||
76 | switch(bom) {
|
||
77 | case 0: |
||
78 | case 15: |
||
79 | case 14: |
||
80 | case 13: |
||
81 | case 12: |
||
82 | follow_direction = 255;
|
||
83 | break;
|
||
84 | case 1: |
||
85 | follow_direction = 150;
|
||
86 | break;
|
||
87 | case 2: |
||
88 | follow_direction = 100;
|
||
89 | break;
|
||
90 | case 3: |
||
91 | follow_direction = 50;
|
||
92 | break;
|
||
93 | case 4: |
||
94 | follow_direction = 0;
|
||
95 | break;
|
||
96 | case 5: |
||
97 | follow_direction = -50;
|
||
98 | break;
|
||
99 | case 6: |
||
100 | follow_direction = -100;
|
||
101 | break;
|
||
102 | case 7: |
||
103 | follow_direction = -150;
|
||
104 | break;
|
||
105 | case 8: |
||
106 | case 9: |
||
107 | case 10: |
||
108 | case 11: |
||
109 | follow_direction = -255;
|
||
110 | break;
|
||
111 | default:
|
||
112 | follow_direction = 0;
|
||
113 | break;
|
||
114 | } |
||
115 | break; // break from loop |
||
116 | } |
||
117 | } |
||
118 | |||
119 | |||
120 | /*If the crazy count is in it's >>3 range, and we are currently a follower, be a leader.*/
|
||
121 | if(cur_state == FOLLOW && crazy_count<=(CRAZY_MAX>>3)) |
||
122 | { |
||
123 | cur_state = LEAD; |
||
124 | } |
||
125 | |||
126 | evaluate_state(); // evaluate state
|
||
127 | } |
||
128 | |||
129 | // create connected components and pick a leader for each chain
|
||
130 | // use modified Prim's alogrithm to find local spanning tree
|
||
131 | // CURRENTLY JUST LOOKS FOR CLOSEST ROBOT TO FOLLOW
|
||
132 | void lemmings_Prims(void) { |
||
133 | |||
134 | int nodeA = wl_get_xbee_id(); // get id of current robot |
||
135 | int nodeB = -1; |
||
136 | int cur_weight = -1; |
||
137 | int best_weight = 1000; // set to infinity initially |
||
138 | int best_node = -1; |
||
139 | |||
140 | // iterate through token ring
|
||
141 | wl_token_iterator_begin(); |
||
142 | while(wl_token_iterator_has_next()) {
|
||
143 | nodeB = wl_token_iterator_next(); |
||
144 | if (nodeB < 0) |
||
145 | break; // no more bots |
||
146 | else if (nodeB == nodeA) |
||
147 | continue; // can't follow self |
||
148 | |||
149 | cur_weight = lemmings_get_edge_weight(nodeA,nodeB); |
||
150 | if (cur_weight < best_weight) {
|
||
151 | // this is new best node, so save values
|
||
152 | best_weight = cur_weight; |
||
153 | best_node = nodeB; |
||
154 | } |
||
155 | } |
||
156 | |||
157 | if (best_node == -1 || (best_weight > 5 && nodeA < nodeB)) { |
||
158 | // no robots to follow, or choosing to be a leader
|
||
159 | cur_state = LEAD; |
||
160 | follow_bot = -1;
|
||
161 | } |
||
162 | else {
|
||
163 | // follow bot
|
||
164 | cur_state = FOLLOW; |
||
165 | follow_bot = nodeB; |
||
166 | } |
||
167 | } |
||
168 | |||
169 | // get edge weight of sensor matrix
|
||
170 | // add in BOM range data when BOM 1.5 comes out
|
||
171 | int lemmings_get_edge_weight(int robot1, int robot2) { |
||
172 | int weight = 0; |
||
173 | int bom = wl_token_get_sensor_reading(robot1,robot2);
|
||
174 | // since we only have positioning data, give better weight to
|
||
175 | // bots in from of us
|
||
176 | switch(bom) {
|
||
177 | case 12: |
||
178 | weight++; |
||
179 | case 13: |
||
180 | case 11: |
||
181 | weight++; |
||
182 | case 14: |
||
183 | case 10: |
||
184 | weight++; |
||
185 | case 15: |
||
186 | case 9: |
||
187 | weight++; |
||
188 | case 0: |
||
189 | case 8: |
||
190 | weight++; |
||
191 | case 1: |
||
192 | case 7: |
||
193 | weight++; |
||
194 | case 2: |
||
195 | case 6: |
||
196 | weight++; |
||
197 | case 3: |
||
198 | case 5: |
||
199 | weight++; |
||
200 | case 4: |
||
201 | break;
|
||
202 | default:
|
||
203 | weight = -1;
|
||
204 | break;
|
||
205 | } |
||
206 | return weight;
|
||
207 | } |
||
208 | |||
209 | |||
210 | //Acts on state change.
|
||
211 | void lemmings_evaluate_state(){
|
||
212 | switch(cur_state){
|
||
213 | case(LEAD):
|
||
214 | run_around_FSM(); // do random run around
|
||
215 | break;
|
||
216 | |||
217 | case(FOLLOW):
|
||
218 | move_avoid(200,follow_direction,33); // move in direction of bot |
||
219 | break;
|
||
220 | |||
221 | default:
|
||
222 | /*Should never get here, go strait.*/
|
||
223 | move(200,0); |
||
224 | break;
|
||
225 | } |
||
226 | } |
||
227 |