root / trunk / code / projects / hunter_prey / smart_run_around_fsm.c @ 1431
History | View | Annotate | Download (3.03 KB)
1 | 1199 | alevkoy | #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 | 1409 | bneuman | state = SRA_FORWARD; // start out moving forward
|
10 | 1199 | alevkoy | // 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 | 1409 | bneuman | state = SRA_SPIN; |
45 | 1199 | alevkoy | /* 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 | 1409 | bneuman | state = SRA_FORWARD; |
50 | 1228 | alevkoy | // avoid left-side collision by turning right
|
51 | else if (d1 < IR13_DANGER || d5 < IR45_DANGER) |
||
52 | 1409 | bneuman | state = SRA_RIGHT; |
53 | 1228 | alevkoy | // avoid right-side collision by turning left
|
54 | else if (d3 < IR13_DANGER || d4 < IR45_DANGER) |
||
55 | 1409 | bneuman | state = SRA_LEFT; |
56 | 1199 | alevkoy | else if (d2 < IR2_INTEREST) // should turn to avoid obstacle up ahead |
57 | { |
||
58 | if (d3 >= d1) // more room on right |
||
59 | 1409 | bneuman | state = SRA_RIGHT; |
60 | 1199 | alevkoy | else // more room on left |
61 | 1409 | bneuman | state = SRA_LEFT; |
62 | 1199 | alevkoy | } |
63 | else // no obstacles close by, so keep going straight |
||
64 | 1409 | bneuman | state = SRA_FORWARD; |
65 | 1199 | alevkoy | |
66 | /* Debugging via USB output */
|
||
67 | 1409 | bneuman | /* 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 | 1199 | alevkoy | |
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 | 1409 | bneuman | case SRA_FORWARD:
|
89 | 1199 | alevkoy | orb_set_color(GREEN); |
90 | 1200 | alevkoy | move(FULL_SPD, 0); // drive straight forward |
91 | 1199 | alevkoy | break;
|
92 | 1409 | bneuman | case SRA_LEFT:
|
93 | 1200 | alevkoy | orbs_set_color(GREEN, YELLOW); // green on left side
|
94 | 1199 | alevkoy | move(HALF_SPD, NRML_TURN); // drive forward and to the left
|
95 | break;
|
||
96 | 1409 | bneuman | case SRA_RIGHT:
|
97 | 1200 | alevkoy | orbs_set_color(YELLOW, GREEN); // green on right side
|
98 | 1199 | alevkoy | move(HALF_SPD, -NRML_TURN); // drive forward and to the right
|
99 | break;
|
||
100 | 1409 | bneuman | case SRA_SPIN:
|
101 | 1199 | alevkoy | 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 | } |