root / demos / john / behavior / main.c @ 1692
History | View | Annotate | Download (9.7 KB)
| 1 | 1500 | jsexton | /*
|
|---|---|---|---|
| 2 | * Hunter-Prey main.c File - Implementation of Hunter-Prey behavior which
|
||
| 3 | * uses finite state machines to manage the behavior. A top level
|
||
| 4 | * state machine controls the high level behavior switches between
|
||
| 5 | * "hunter" and "prey" and manages the wireless communication. Two
|
||
| 6 | * additional state machines control the behavior of the robot when
|
||
| 7 | * it is in "prey" mode and when it is in "hunter" mode.
|
||
| 8 | *
|
||
| 9 | * Author: John Sexton, Colony Project, CMU Robotics Club
|
||
| 10 | */
|
||
| 11 | |||
| 12 | 1486 | jsexton | #include <dragonfly_lib.h> |
| 13 | #include <wl_basic.h> |
||
| 14 | #include "hunter_prey.h" |
||
| 15 | #include "encoders.h" |
||
| 16 | |||
| 17 | 1644 | jsexton | #define WL_CHANNEL 24 |
| 18 | 1486 | jsexton | |
| 19 | 1632 | jsexton | #define BACK_THRESHOLD -1000 |
| 20 | 1486 | jsexton | #define TURN_DIST 1024 |
| 21 | 1640 | jsexton | #define IR_DIST_THRESHOLD 150 |
| 22 | 1632 | jsexton | #define WAIT_DELAY_MS 2000 |
| 23 | 1486 | jsexton | |
| 24 | /* State Macros */
|
||
| 25 | |||
| 26 | 1500 | jsexton | /* Top Level FSM States */
|
| 27 | 1486 | jsexton | #define TOP_INIT 0 |
| 28 | #define TOP_HUNTER_HUNT 1 |
||
| 29 | #define TOP_HUNTER_TAG 2 |
||
| 30 | #define TOP_HUNTER_PURSUE 3 |
||
| 31 | #define TOP_PREY_AVOID 4 |
||
| 32 | #define TOP_HUNTER_WAIT 5 |
||
| 33 | #define TOP_ERROR 6 |
||
| 34 | |||
| 35 | 1500 | jsexton | /* Hunter FSM States */
|
| 36 | 1486 | jsexton | #define HUNTER_SPIRAL 0 |
| 37 | #define HUNTER_CHASE 1 |
||
| 38 | |||
| 39 | 1500 | jsexton | /* Prey FSM States */
|
| 40 | 1486 | jsexton | #define PREY_START_BACK 0 |
| 41 | #define PREY_BACKING 1 |
||
| 42 | #define PREY_TURN 2 |
||
| 43 | #define PREY_AVOID 3 |
||
| 44 | |||
| 45 | |||
| 46 | /* Function prototype declarations */
|
||
| 47 | int hunter_FSM(int, int, int); |
||
| 48 | int prey_FSM(int); |
||
| 49 | |||
| 50 | /* Variables used to receive packets */
|
||
| 51 | unsigned char* packet_data; |
||
| 52 | int data_length;
|
||
| 53 | |||
| 54 | /* Data buffer used to send packets */
|
||
| 55 | 1652 | jsexton | char send_buffer[2]; |
| 56 | 1486 | jsexton | |
| 57 | int main(void) |
||
| 58 | {
|
||
| 59 | |||
| 60 | /* Initialize dragonfly board */
|
||
| 61 | dragonfly_init(ALL_ON); |
||
| 62 | xbee_init(); |
||
| 63 | encoders_init(); |
||
| 64 | |||
| 65 | /* Initialize the basic wireless library */
|
||
| 66 | wl_basic_init_default(); |
||
| 67 | |||
| 68 | /* Set the XBee channel to assigned channel */
|
||
| 69 | wl_set_channel(WL_CHANNEL); |
||
| 70 | |||
| 71 | |||
| 72 | /* ****** CODE HERE ******* */
|
||
| 73 | |||
| 74 | /* Initialize state machines */
|
||
| 75 | int state = TOP_INIT;
|
||
| 76 | int hunter_state = HUNTER_SPIRAL;
|
||
| 77 | int prey_state = PREY_AVOID;
|
||
| 78 | |||
| 79 | int frontIR = 0; |
||
| 80 | int maxBOM = 0; |
||
| 81 | int robotID = get_robotid();
|
||
| 82 | int oldTime = 0, curTime = 0; |
||
| 83 | |||
| 84 | while (1) { |
||
| 85 | |||
| 86 | 1500 | jsexton | /* Check if we've received a wireless packet */
|
| 87 | 1486 | jsexton | packet_data = wl_basic_do_default(&data_length); |
| 88 | |||
| 89 | 1500 | jsexton | /* Top level state machines */
|
| 90 | 1486 | jsexton | switch(state) {
|
| 91 | |||
| 92 | case TOP_INIT:
|
||
| 93 | orbs_set_color(RED, GREEN); |
||
| 94 | delay_ms(500);
|
||
| 95 | orbs_set_color(GREEN, RED); |
||
| 96 | delay_ms(500);
|
||
| 97 | |||
| 98 | 1500 | jsexton | /* Allow user to pick the starting behavior */
|
| 99 | 1486 | jsexton | if (button1_read()) {
|
| 100 | state = TOP_PREY_AVOID; |
||
| 101 | prey_state = PREY_AVOID; |
||
| 102 | } else {
|
||
| 103 | state = TOP_HUNTER_HUNT; |
||
| 104 | hunter_state = HUNTER_SPIRAL; |
||
| 105 | } |
||
| 106 | break;
|
||
| 107 | case TOP_HUNTER_HUNT:
|
||
| 108 | orbs_set_color(RED, RED); |
||
| 109 | |||
| 110 | if (packet_data && data_length == 2 |
||
| 111 | && packet_data[0] == HUNTER_PREY_ACTION_ACK) {
|
||
| 112 | 1500 | jsexton | /* If we've received an ACK, we need to wait */
|
| 113 | 1486 | jsexton | state = TOP_HUNTER_WAIT; |
| 114 | } else {
|
||
| 115 | 1500 | jsexton | /* Record some sensor readings and check if we can TAG */
|
| 116 | 1486 | jsexton | bom_refresh(BOM_ALL); |
| 117 | frontIR = range_read_distance(IR2); |
||
| 118 | maxBOM = get_max_bom(); |
||
| 119 | 1640 | jsexton | if (hunter_prey_tagged(maxBOM, frontIR)) {
|
| 120 | 1486 | jsexton | state = TOP_HUNTER_TAG; |
| 121 | } else {
|
||
| 122 | 1500 | jsexton | /* If we haven't tagged, then enter hunter FSM */
|
| 123 | 1486 | jsexton | hunter_state = hunter_FSM(hunter_state, maxBOM, frontIR); |
| 124 | } |
||
| 125 | } |
||
| 126 | break;
|
||
| 127 | case TOP_HUNTER_TAG:
|
||
| 128 | orbs_set_color(RED, PURPLE); |
||
| 129 | |||
| 130 | if (packet_data && data_length == 2 |
||
| 131 | && packet_data[0] == HUNTER_PREY_ACTION_ACK) {
|
||
| 132 | 1500 | jsexton | /* If we've received an ACK, then someone beat us to the TAG and
|
| 133 | * we need to wait. */
|
||
| 134 | 1486 | jsexton | state = TOP_HUNTER_WAIT; |
| 135 | } else {
|
||
| 136 | 1500 | jsexton | /* Prepare and send the TAG packet */
|
| 137 | 1486 | jsexton | send_buffer[0] = HUNTER_PREY_ACTION_TAG;
|
| 138 | send_buffer[1] = robotID;
|
||
| 139 | wl_basic_send_global_packet(42, send_buffer, 2); |
||
| 140 | |||
| 141 | 1500 | jsexton | /* Record the time so we don't spam a TAG message on the network */
|
| 142 | 1486 | jsexton | oldTime = rtc_get(); |
| 143 | state = TOP_HUNTER_PURSUE; |
||
| 144 | } |
||
| 145 | break;
|
||
| 146 | case TOP_HUNTER_PURSUE:
|
||
| 147 | orbs_set_color(RED, BLUE); |
||
| 148 | curTime = rtc_get(); |
||
| 149 | |||
| 150 | if (packet_data && data_length == 2 |
||
| 151 | && packet_data[0] == HUNTER_PREY_ACTION_ACK) {
|
||
| 152 | 1500 | jsexton | /* Check if we've received a new wireless packet */
|
| 153 | 1486 | jsexton | |
| 154 | if (packet_data[1] == robotID) { |
||
| 155 | 1500 | jsexton | /* We've been ACKed, so we can now become the prey */
|
| 156 | 1486 | jsexton | state = TOP_PREY_AVOID; |
| 157 | prey_state = PREY_START_BACK; |
||
| 158 | } else {
|
||
| 159 | 1500 | jsexton | /* If we get an ACK with a different robotID, then someone beat us
|
| 160 | * to the TAG, so we must wait */
|
||
| 161 | 1486 | jsexton | state = TOP_HUNTER_WAIT; |
| 162 | } |
||
| 163 | |||
| 164 | } else if (curTime - oldTime > 1) { |
||
| 165 | 1500 | jsexton | /* If 1 second has ellapsed, return to normal hunting state (we can
|
| 166 | * TAG again now) */
|
||
| 167 | 1486 | jsexton | state = TOP_HUNTER_HUNT; |
| 168 | 1500 | jsexton | } else if (oldTime > curTime) { |
| 169 | /* If for some reason the timer overflows, or the wireless library
|
||
| 170 | * (which is also using the same timer) resets the timer,
|
||
| 171 | * reinitialize the timer so that we don't wait too long for the
|
||
| 172 | * timer to catch back up. */
|
||
| 173 | 1486 | jsexton | oldTime = curTime; |
| 174 | } else {
|
||
| 175 | 1500 | jsexton | /* If no other behavioral changes need to be made, then continue
|
| 176 | * with the hunter FSM where we left off */
|
||
| 177 | 1486 | jsexton | bom_refresh(BOM_ALL); |
| 178 | frontIR = range_read_distance(IR2); |
||
| 179 | maxBOM = get_max_bom(); |
||
| 180 | hunter_state = hunter_FSM(hunter_state, maxBOM, frontIR); |
||
| 181 | } |
||
| 182 | break;
|
||
| 183 | case TOP_PREY_AVOID:
|
||
| 184 | orbs_set_color(GREEN, GREEN); |
||
| 185 | if (packet_data && data_length == 2 |
||
| 186 | && packet_data[0] == HUNTER_PREY_ACTION_TAG) {
|
||
| 187 | 1500 | jsexton | /* Check if we've received a TAG yet. If so then send an ACK back */
|
| 188 | 1486 | jsexton | |
| 189 | send_buffer[0] = HUNTER_PREY_ACTION_ACK;
|
||
| 190 | send_buffer[1] = packet_data[1]; |
||
| 191 | wl_basic_send_global_packet(42, send_buffer, 2); |
||
| 192 | |||
| 193 | state = TOP_HUNTER_WAIT; |
||
| 194 | } else {
|
||
| 195 | 1500 | jsexton | /* If we haven't received a TAG yet, continue with prey FSM */
|
| 196 | 1486 | jsexton | bom_on(); |
| 197 | prey_state = prey_FSM(prey_state); |
||
| 198 | } |
||
| 199 | break;
|
||
| 200 | case TOP_HUNTER_WAIT:
|
||
| 201 | 1500 | jsexton | /* Set orb colors and wait to give the prey the 5 second head start */
|
| 202 | 1486 | jsexton | orbs_set_color(BLUE, BLUE); |
| 203 | 1501 | jsexton | bom_off(); |
| 204 | motors_off(); |
||
| 205 | 1632 | jsexton | delay_ms(WAIT_DELAY_MS); |
| 206 | 1486 | jsexton | state = TOP_HUNTER_HUNT; |
| 207 | hunter_state = HUNTER_SPIRAL; |
||
| 208 | break;
|
||
| 209 | case TOP_ERROR:
|
||
| 210 | default:
|
||
| 211 | orbs_set_color(PURPLE, PURPLE); |
||
| 212 | state = TOP_ERROR; |
||
| 213 | while(1); |
||
| 214 | break;
|
||
| 215 | } |
||
| 216 | |||
| 217 | } |
||
| 218 | |||
| 219 | /* ****** END HERE ******* */
|
||
| 220 | |||
| 221 | while(1); |
||
| 222 | |||
| 223 | return 0; |
||
| 224 | |||
| 225 | } |
||
| 226 | |||
| 227 | 1500 | jsexton | |
| 228 | /*
|
||
| 229 | * prey_FSM - Prey finite state machine which starts by backing away, turning,
|
||
| 230 | * and then running and avoiding obstacles.
|
||
| 231 | *
|
||
| 232 | * Arguments:
|
||
| 233 | * prey_state - Current prey state.
|
||
| 234 | *
|
||
| 235 | * returns - The new state of the prey state machine.
|
||
| 236 | */
|
||
| 237 | |||
| 238 | 1486 | jsexton | int prey_FSM(int prey_state) { |
| 239 | |||
| 240 | 1500 | jsexton | /* Variable to store the front rangefinder readings */
|
| 241 | 1486 | jsexton | int rangeVals[3] = {0, 0, 0}; |
| 242 | 1500 | jsexton | |
| 243 | 1486 | jsexton | switch (prey_state) {
|
| 244 | |||
| 245 | case PREY_START_BACK:
|
||
| 246 | motor_l_set(BACKWARD, 255);
|
||
| 247 | motor_r_set(BACKWARD, 255);
|
||
| 248 | encoder_rst_dx(LEFT); |
||
| 249 | 1640 | jsexton | encoder_rst_dx(RIGHT); |
| 250 | 1486 | jsexton | return PREY_BACKING;
|
| 251 | break;
|
||
| 252 | case PREY_BACKING:
|
||
| 253 | 1640 | jsexton | if (encoder_get_x(LEFT) < BACK_THRESHOLD
|
| 254 | || encoder_get_x(RIGHT) < BACK_THRESHOLD) {
|
||
| 255 | 1486 | jsexton | motor_l_set(BACKWARD, 255);
|
| 256 | motor_r_set(FORWARD, 255);
|
||
| 257 | 1640 | jsexton | encoder_rst_dx(LEFT); |
| 258 | 1486 | jsexton | encoder_rst_dx(RIGHT); |
| 259 | return PREY_TURN;
|
||
| 260 | } else {
|
||
| 261 | return PREY_BACKING;
|
||
| 262 | } |
||
| 263 | break;
|
||
| 264 | case PREY_TURN:
|
||
| 265 | 1640 | jsexton | if (encoder_get_x(LEFT) < -TURN_DIST
|
| 266 | || encoder_get_x(RIGHT) > TURN_DIST) {
|
||
| 267 | 1486 | jsexton | return PREY_AVOID;
|
| 268 | } else {
|
||
| 269 | return PREY_TURN;
|
||
| 270 | } |
||
| 271 | break;
|
||
| 272 | case PREY_AVOID:
|
||
| 273 | rangeVals[0] = range_read_distance(IR1);
|
||
| 274 | rangeVals[1] = range_read_distance(IR2);
|
||
| 275 | rangeVals[2] = range_read_distance(IR3);
|
||
| 276 | |||
| 277 | 1500 | jsexton | /* Drive away if we detect obstacles using the rangefinders */
|
| 278 | 1486 | jsexton | if (rangeVals[1] > 0 && rangeVals[1] < IR_DIST_THRESHOLD) { |
| 279 | 1643 | jsexton | if (rangeVals[0] < rangeVals[2]) { |
| 280 | motor_l_set(FORWARD, 255);
|
||
| 281 | motor_r_set(BACKWARD, 255);
|
||
| 282 | } else {
|
||
| 283 | motor_l_set(BACKWARD, 255);
|
||
| 284 | motor_r_set(FORWARD, 255);
|
||
| 285 | } |
||
| 286 | 1486 | jsexton | return PREY_AVOID;
|
| 287 | } else if (rangeVals[0] > 0 && rangeVals[0] < IR_DIST_THRESHOLD) { |
||
| 288 | motor_l_set(FORWARD, 255);
|
||
| 289 | motor_r_set(FORWARD, 170);
|
||
| 290 | return PREY_AVOID;
|
||
| 291 | } else if (rangeVals[2] > 0 && rangeVals[2] < IR_DIST_THRESHOLD) { |
||
| 292 | motor_l_set(FORWARD, 170);
|
||
| 293 | motor_r_set(FORWARD, 255);
|
||
| 294 | return PREY_AVOID;
|
||
| 295 | } else {
|
||
| 296 | motor_l_set(FORWARD, 255);
|
||
| 297 | motor_r_set(FORWARD, 255);
|
||
| 298 | return PREY_AVOID;
|
||
| 299 | } |
||
| 300 | break;
|
||
| 301 | default:
|
||
| 302 | return PREY_AVOID;
|
||
| 303 | break;
|
||
| 304 | |||
| 305 | } |
||
| 306 | |||
| 307 | return prey_state;
|
||
| 308 | |||
| 309 | } |
||
| 310 | |||
| 311 | 1500 | jsexton | |
| 312 | /*
|
||
| 313 | * hunter_FSM - Hunter finite state machine which defaults to spiraling
|
||
| 314 | * outwards until the BOM can locate the prey. Once the BOM locates
|
||
| 315 | * the prey, chase the prey as fast as possible.
|
||
| 316 | *
|
||
| 317 | * Arguments:
|
||
| 318 | * hunter_state - Current hunter state.
|
||
| 319 | * maxBOM - Current maximum BOM value.
|
||
| 320 | * frontIR - Current front IR rangefinder reading value.
|
||
| 321 | *
|
||
| 322 | * returns - The new state of the hunter state machine.
|
||
| 323 | */
|
||
| 324 | |||
| 325 | 1486 | jsexton | int hunter_FSM(int hunter_state, int maxBOM, int frontIR) { |
| 326 | |||
| 327 | switch(hunter_state) {
|
||
| 328 | |||
| 329 | case HUNTER_SPIRAL:
|
||
| 330 | if (maxBOM != -1) { |
||
| 331 | return HUNTER_CHASE;
|
||
| 332 | } else {
|
||
| 333 | motor_l_set(FORWARD, 170);
|
||
| 334 | motor_r_set(FORWARD, 190);
|
||
| 335 | return HUNTER_SPIRAL;
|
||
| 336 | } |
||
| 337 | break;
|
||
| 338 | case HUNTER_CHASE:
|
||
| 339 | 1643 | jsexton | |
| 340 | 1486 | jsexton | if (maxBOM == -1) { |
| 341 | return HUNTER_CHASE;
|
||
| 342 | } else if (maxBOM == 4) { |
||
| 343 | motor_l_set(FORWARD, 255);
|
||
| 344 | motor_r_set(FORWARD, 255);
|
||
| 345 | return HUNTER_CHASE;
|
||
| 346 | } else if (maxBOM == 3) { |
||
| 347 | motor_l_set(FORWARD, 255);
|
||
| 348 | motor_r_set(FORWARD, 240);
|
||
| 349 | return HUNTER_CHASE;
|
||
| 350 | } else if (maxBOM == 5) { |
||
| 351 | motor_l_set(FORWARD, 240);
|
||
| 352 | motor_r_set(FORWARD, 255);
|
||
| 353 | return HUNTER_CHASE;
|
||
| 354 | } else if (maxBOM < 3) { |
||
| 355 | motor_l_set(FORWARD, 255);
|
||
| 356 | motor_r_set(FORWARD, 170);
|
||
| 357 | return HUNTER_CHASE;
|
||
| 358 | } else if (maxBOM > 5 && maxBOM <= 8) { |
||
| 359 | motor_l_set(FORWARD, 170);
|
||
| 360 | motor_r_set(FORWARD, 255);
|
||
| 361 | return HUNTER_CHASE;
|
||
| 362 | } else if (maxBOM > 8 && maxBOM < 12) { |
||
| 363 | motor_l_set(BACKWARD, 255);
|
||
| 364 | motor_r_set(FORWARD, 255);
|
||
| 365 | return HUNTER_CHASE;
|
||
| 366 | } else {
|
||
| 367 | motor_l_set(FORWARD, 255);
|
||
| 368 | motor_r_set(BACKWARD, 255);
|
||
| 369 | return HUNTER_CHASE;
|
||
| 370 | } |
||
| 371 | break;
|
||
| 372 | default:
|
||
| 373 | return HUNTER_SPIRAL;
|
||
| 374 | break;
|
||
| 375 | |||
| 376 | } |
||
| 377 | |||
| 378 | return hunter_state;
|
||
| 379 | |||
| 380 | } |