Revision 212
update lemmings branch
updated smart_run_around code to latest version
for lemmings_dsschult:
leader state works
follower state has sensor matrix and/or bom issues
suggest test when no one else is using wireless
lemmings.c | ||
---|---|---|
2 | 2 |
#include <wl_defs.h> |
3 | 3 |
#include <wireless.h> |
4 | 4 |
#include <wl_token_ring.h> |
5 |
//#include "smart_run_around_fsm.h"
|
|
5 |
#include "smart_run_around_fsm.h" |
|
6 | 6 |
#include "lemmings.h" |
7 | 7 |
|
8 | 8 |
/*A simple behavior for following the leader. |
... | ... | |
13 | 13 |
|
14 | 14 |
void lemmings_init() |
15 | 15 |
{ |
16 |
//run_around_init(); // prepare for moving |
|
17 | 16 |
dragonfly_init(ALL_ON); |
17 |
run_around_init(); // prepare for moving |
|
18 | 18 |
analog_init(); |
19 | 19 |
motors_init(); |
20 | 20 |
range_init(); |
... | ... | |
55 | 55 |
|
56 | 56 |
/*The main function, call this to update states as frequently as possible.*/ |
57 | 57 |
void lemmings_FSM(void) { |
58 |
int bom=0;
|
|
58 |
int bom=-1;
|
|
59 | 59 |
int i=0; |
60 | 60 |
int old_follow_bot; |
61 | 61 |
|
... | ... | |
63 | 63 |
if (follow_bot < 0) |
64 | 64 |
lemmings_Prims(); // if we are not following, then attempt to follow |
65 | 65 |
|
66 |
usb_puts("following: "); |
|
67 |
usb_puti(follow_bot); |
|
68 |
orb_set_color(GREEN); |
|
69 | 66 |
// if we are following a bot, make sure we can still see it |
70 | 67 |
old_follow_bot = follow_bot; |
71 | 68 |
for (i=0;follow_bot>=0&&i<10;i++) { |
... | ... | |
130 | 127 |
cur_state = LEAD; |
131 | 128 |
}*/ |
132 | 129 |
|
130 |
usb_puts("following: "); |
|
131 |
usb_puti(follow_bot); |
|
132 |
usb_puts("bom: "); |
|
133 |
usb_puti(bom); |
|
134 |
usb_puts("\n\r"); |
|
135 |
|
|
133 | 136 |
lemmings_evaluate_state(); // evaluate state |
134 | 137 |
} |
135 | 138 |
|
... | ... | |
159 | 162 |
continue; // can't follow self |
160 | 163 |
|
161 | 164 |
cur_weight = lemmings_get_edge_weight(nodeA,nodeB); |
162 |
if (cur_weight < best_weight) { |
|
165 |
usb_puts("weight: "); |
|
166 |
usb_puti(cur_weight); |
|
167 |
usb_puts("\n\r"); |
|
168 |
if (cur_weight >= 0 && cur_weight < best_weight) { |
|
163 | 169 |
// this is new best node, so save values |
164 | 170 |
best_weight = cur_weight; |
165 | 171 |
best_node = nodeB; |
... | ... | |
226 | 232 |
usb_puts("\n\r"); |
227 | 233 |
switch(cur_state){ |
228 | 234 |
case(LEAD): |
229 |
move_avoid(200,0,50); |
|
230 |
orb_set_color(GREEN); |
|
231 |
//run_around_FSM(); // do random run around
|
|
235 |
//move_avoid(200,0,50);
|
|
236 |
//orb_set_color(GREEN);
|
|
237 |
run_around_FSM(); // do random run around |
|
232 | 238 |
break; |
233 | 239 |
|
234 | 240 |
case(FOLLOW): |
Also available in: Unified diff