Project

General

Profile

Revision 678

Added by David Schultz about 16 years ago

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:

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();

Also available in: Unified diff