root / trunk / code / projects / hunter_prey / smart_run_around_fsm.c @ 1447
History | View | Annotate | Download (3 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 = SRA_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 = SRA_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 = SRA_FORWARD; |
| 50 | // avoid left-side collision by turning right
|
| 51 | else if (d1 < IR13_DANGER || d5 < IR45_DANGER) |
| 52 | state = SRA_RIGHT; |
| 53 | // avoid right-side collision by turning left
|
| 54 | else if (d3 < IR13_DANGER || d4 < IR45_DANGER) |
| 55 | state = SRA_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 = SRA_RIGHT; |
| 60 | else // more room on left |
| 61 | state = SRA_LEFT; |
| 62 | } |
| 63 | else // no obstacles close by, so keep going straight |
| 64 | state = SRA_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 SRA_FORWARD: |
| 89 | orb_set_color(GREEN); |
| 90 | move(220, 0); // drive straight forward |
| 91 | break;
|
| 92 | case SRA_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 SRA_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 SRA_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 |