root / trunk / code / projects / mapping / auto / smart_run_around_fsm.c @ 1201
History | View | Annotate | Download (2.95 KB)
1 | 1201 | alevkoy | #include <dragonfly_lib.h> |
---|---|---|---|
2 | 722 | justin | #include "smart_run_around_fsm.h" |
3 | |||
4 | void run_around_init(void) |
||
5 | { |
||
6 | 1162 | alevkoy | range_init(); |
7 | 1201 | alevkoy | orb_init(); |
8 | 1162 | alevkoy | |
9 | 1201 | alevkoy | state = FORWARD; // start out moving forward
|
10 | // initialize rangefinder values to 0
|
||
11 | d1 = 0, d2 = 0, d3 = 0, d4 = 0, d5 = 0; |
||
12 | 722 | justin | } |
13 | |||
14 | 1201 | alevkoy | /* 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 | 1162 | alevkoy | |
27 | 1201 | alevkoy | temp = range_read_distance(IR1); |
28 | d1 = (temp == -1) ? d1 : temp;
|
||
29 | 722 | justin | |
30 | 1201 | alevkoy | temp = range_read_distance(IR2); |
31 | d2 = (temp == -1) ? d2 : temp;
|
||
32 | 722 | justin | |
33 | 1201 | alevkoy | temp = range_read_distance(IR3); |
34 | d3 = (temp == -1) ? d3 : temp;
|
||
35 | 722 | justin | |
36 | 1201 | alevkoy | 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 | else if (d1 < IR13_DANGER) // avoid left-side collision by turning right |
||
51 | state = RIGHT; |
||
52 | else if (d3 < IR13_DANGER) // avoid right-side collision by turning left |
||
53 | state = LEFT; |
||
54 | else if (d2 < IR2_INTEREST) // should turn to avoid obstacle up ahead |
||
55 | { |
||
56 | if (d3 >= d1) // more room on right |
||
57 | state = RIGHT; |
||
58 | else // more room on left |
||
59 | state = LEFT; |
||
60 | } |
||
61 | else // no obstacles close by, so keep going straight |
||
62 | state = FORWARD; |
||
63 | |||
64 | /* Debugging via USB output */
|
||
65 | usb_puts("IR1: ");
|
||
66 | usb_puti(d1); |
||
67 | usb_puts(" IR2: ");
|
||
68 | usb_puti(d2); |
||
69 | usb_puts(" IR3: ");
|
||
70 | usb_puti(d3); |
||
71 | usb_puts(" IR4: ");
|
||
72 | usb_puti(d4); |
||
73 | usb_puts(" IR5: ");
|
||
74 | usb_puti(d5); |
||
75 | usb_puts("\n\r");
|
||
76 | |||
77 | if (button2_read()) // pushing buttons resets robot |
||
78 | reset(); |
||
79 | |||
80 | evaluate_state(); // take action on updated state
|
||
81 | 722 | justin | } |
82 | |||
83 | 1201 | alevkoy | // behave according to current state
|
84 | // TODO: adjust speeds after testing
|
||
85 | void evaluate_state(void) |
||
86 | { |
||
87 | switch (state)
|
||
88 | { |
||
89 | case FORWARD:
|
||
90 | orb_set_color(GREEN); |
||
91 | move(FULL_SPD, 0); // drive straight forward |
||
92 | break;
|
||
93 | case LEFT:
|
||
94 | orbs_set_color(GREEN, YELLOW); // green on left side
|
||
95 | move(HALF_SPD, NRML_TURN); // drive forward and to the left
|
||
96 | break;
|
||
97 | case RIGHT:
|
||
98 | orbs_set_color(YELLOW, GREEN); // green on right side
|
||
99 | move(HALF_SPD, -NRML_TURN); // drive forward and to the right
|
||
100 | break;
|
||
101 | case SPIN:
|
||
102 | orb_set_color(RED); |
||
103 | move(0, NRML_TURN); // spin CCW without traveling |
||
104 | break;
|
||
105 | default: // should never reach this |
||
106 | orb_set_color(PURPLE); |
||
107 | move(0, 0); // stop completely |
||
108 | } |
||
109 | } |