Project

General

Profile

Statistics
| Revision:

root / trunk / code / projects / hunter_prey / abenico / smart_run_around_fsm.c @ 1464

History | View | Annotate | Download (3.03 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