Project

General

Profile

Revision 678

bfs for multiple seekers works as long as they all have line of sight. bfs without line of sight needs work in the transition from the followed robot to the target robot.

View differences:

trunk/code/behaviors/bfs_fsm/bfs_fsm.c
59 59
  usb_puts("\n\rwl_do");
60 60
  usb_puts("|state=");
61 61
  usb_puti(bfs_state);
62
  usb_puts("|bfs_follow=");
63
  usb_puti(bfs_follow_id);
62 64
  usb_puts("|");
63 65
  
64 66
    
......
69 71
    if (bfs_follow_id != BFS_NO_VAL && bfs_follow_id < BFS_MAX_ROBOTS) {
70 72
      bfs_state = BFS_FOLLOW; // move to follow state
71 73
      bfs_d2=1000;
74
      bfs_check_id=0;
72 75
    }
73 76
  }
74 77
  if (bfs_state == BFS_FOLLOW) {
78
    /* check robot to follow */
79
    if (++bfs_check_id > BFS_CHECK_ID_TIME) {
80
      bfs_check_id=0;
81
      temp = bfs_follow();
82
      wl_do();
83
      if (temp == BFS_NO_VAL) {
84
        bfs_state = BFS_SEEK;
85
        return;
86
      } else if (temp != bfs_follow_id)
87
        bfs_follow_id = temp;
88
    }
75 89
    // get bom reading
76 90
    temp = wl_token_get_my_sensor_reading(bfs_follow_id);
77 91
    if (temp == -1 || temp > 16) {
......
109 123
  }
110 124
  if (bfs_state == BFS_ORBIT) {   
111 125
    usb_puts("orbiting\n");
126
    orbit_state = ORBIT_INSERTION;
112 127
    while(orbit_state != ORBIT_STOP)
113 128
    { orbit_fsm(); }
114 129
    bfs_state = BFS_STOP; // move to stop state  
115 130
  }
131
  if (bfs_state == BFS_STOP && bfs_follow_id != bfs_otherRobot) {
132
    bfs_state = BFS_SEEK;
133
    return;
134
  }
116 135
  
117 136
  // evaluate state
118 137
  bfs_evaluate_state();
trunk/code/behaviors/bfs_fsm/bfs_fsm.h
21 21

  
22 22
#define BFS_NO_VAL 255
23 23

  
24
#define BFS_CHECK_ID_TIME 100
24 25

  
25 26

  
27

  
26 28
int bfs_state;    /*State machine variable.*/
27 29

  
28 30
int bfs_otherRobot; /* the robot we are seeking */
29 31
int bfs_my_id; /* my wireless id */
30 32
int bfs_follow_id; /* robot to follow */
33
int bfs_check_id; /* timer to check robot id to follow */
31 34

  
32 35

  
33 36
int bfs_pControl;		/*Proportional control variable, determines turn direction.*/

Also available in: Unified diff