root / trunk / code / projects / mapping / new_run_around / smart_run_around_fsm.c @ 1228
History | View | Annotate | Download (2.92 KB)
1 |
#include <dragonfly_lib.h> |
---|---|
2 |
#include "smart_run_around_fsm.h" |
3 |
|
4 |
void run_around_init(void) |
5 |
{ |
6 |
range_init(); |
7 |
orb_init(); |
8 |
|
9 |
state = FORWARD; // start out moving forward
|
10 |
// initialize rangefinder values to 0
|
11 |
d1 = 0, d2 = 0, d3 = 0, d4 = 0, d5 = 0; |
12 |
} |
13 |
|
14 |
/* evaluate rangefinder input and update state
|
15 |
* call this function as often as possible to avoid collisions
|
16 |
*/
|
17 |
void run_around_FSM(void) |
18 |
{ |
19 |
/* TODO: find a better way to handle rangefinder input
|
20 |
* robot should deal with -1s (obstacles too close or too far to detect)
|
21 |
* in a way that keeps it from crashing or driving in circles
|
22 |
*/
|
23 |
// do not update distances when rangefinders return -1
|
24 |
// otherwise update distances with new rangefinder values
|
25 |
int16_t temp; |
26 |
|
27 |
temp = range_read_distance(IR1); |
28 |
d1 = (temp == -1) ? d1 : temp;
|
29 |
|
30 |
temp = range_read_distance(IR2); |
31 |
d2 = (temp == -1) ? d2 : temp;
|
32 |
|
33 |
temp = range_read_distance(IR3); |
34 |
d3 = (temp == -1) ? d3 : temp;
|
35 |
|
36 |
temp = range_read_distance(IR4); |
37 |
d4 = (temp == -1) ? d4 : temp;
|
38 |
|
39 |
temp = range_read_distance(IR5); |
40 |
d5 = (temp == -1) ? d5 : temp;
|
41 |
|
42 |
// update state based on rangefinder input
|
43 |
if (d2 < IR2_DANGER) // avoid frontal collision by turning in place |
44 |
state = SPIN; |
45 |
/* nowhere to turn, so don't
|
46 |
* will probably need to turn around soon, but not yet
|
47 |
*/
|
48 |
else if (d1 < IR13_DANGER && d3 < IR13_DANGER) |
49 |
state = FORWARD; |
50 |
// avoid left-side collision by turning right
|
51 |
else if (d1 < IR13_DANGER || d5 < IR45_DANGER) |
52 |
state = RIGHT; |
53 |
// avoid right-side collision by turning left
|
54 |
else if (d3 < IR13_DANGER || d4 < IR45_DANGER) |
55 |
state = LEFT; |
56 |
else if (d2 < IR2_INTEREST) // should turn to avoid obstacle up ahead |
57 |
{ |
58 |
if (d3 >= d1) // more room on right |
59 |
state = RIGHT; |
60 |
else // more room on left |
61 |
state = LEFT; |
62 |
} |
63 |
else // no obstacles close by, so keep going straight |
64 |
state = FORWARD; |
65 |
|
66 |
/* Debugging via USB output */
|
67 |
usb_puts("IR1: ");
|
68 |
usb_puti(d1); |
69 |
usb_puts(" IR2: ");
|
70 |
usb_puti(d2); |
71 |
usb_puts(" IR3: ");
|
72 |
usb_puti(d3); |
73 |
usb_puts(" IR4: ");
|
74 |
usb_puti(d4); |
75 |
usb_puts(" IR5: ");
|
76 |
usb_puti(d5); |
77 |
usb_puts("\n\r");
|
78 |
|
79 |
evaluate_state(); // take action on updated state
|
80 |
} |
81 |
|
82 |
// behave according to current state
|
83 |
// TODO: adjust speeds after testing
|
84 |
void evaluate_state(void) |
85 |
{ |
86 |
switch (state)
|
87 |
{ |
88 |
case FORWARD:
|
89 |
orb_set_color(GREEN); |
90 |
move(FULL_SPD, 0); // drive straight forward |
91 |
break;
|
92 |
case LEFT:
|
93 |
orbs_set_color(GREEN, YELLOW); // green on left side
|
94 |
move(HALF_SPD, NRML_TURN); // drive forward and to the left
|
95 |
break;
|
96 |
case RIGHT:
|
97 |
orbs_set_color(YELLOW, GREEN); // green on right side
|
98 |
move(HALF_SPD, -NRML_TURN); // drive forward and to the right
|
99 |
break;
|
100 |
case SPIN:
|
101 |
orb_set_color(RED); |
102 |
move(0, NRML_TURN); // spin CCW without traveling |
103 |
break;
|
104 |
default: // should never reach this |
105 |
orb_set_color(PURPLE); |
106 |
move(0, 0); // stop completely |
107 |
} |
108 |
} |
109 |
|