root / demos / john / behavior / main.c @ 1692
History | View | Annotate | Download (9.7 KB)
| 1 | /*
|
|---|---|
| 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 | #include <dragonfly_lib.h> |
| 13 | #include <wl_basic.h> |
| 14 | #include "hunter_prey.h" |
| 15 | #include "encoders.h" |
| 16 | |
| 17 | #define WL_CHANNEL 24 |
| 18 | |
| 19 | #define BACK_THRESHOLD -1000 |
| 20 | #define TURN_DIST 1024 |
| 21 | #define IR_DIST_THRESHOLD 150 |
| 22 | #define WAIT_DELAY_MS 2000 |
| 23 | |
| 24 | /* State Macros */
|
| 25 | |
| 26 | /* Top Level FSM States */
|
| 27 | #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 | /* Hunter FSM States */
|
| 36 | #define HUNTER_SPIRAL 0 |
| 37 | #define HUNTER_CHASE 1 |
| 38 | |
| 39 | /* Prey FSM States */
|
| 40 | #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 | char send_buffer[2]; |
| 56 | |
| 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 | /* Check if we've received a wireless packet */
|
| 87 | packet_data = wl_basic_do_default(&data_length); |
| 88 | |
| 89 | /* Top level state machines */
|
| 90 | 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 | /* Allow user to pick the starting behavior */
|
| 99 | 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 | /* If we've received an ACK, we need to wait */
|
| 113 | state = TOP_HUNTER_WAIT; |
| 114 | } else {
|
| 115 | /* Record some sensor readings and check if we can TAG */
|
| 116 | bom_refresh(BOM_ALL); |
| 117 | frontIR = range_read_distance(IR2); |
| 118 | maxBOM = get_max_bom(); |
| 119 | if (hunter_prey_tagged(maxBOM, frontIR)) {
|
| 120 | state = TOP_HUNTER_TAG; |
| 121 | } else {
|
| 122 | /* If we haven't tagged, then enter hunter FSM */
|
| 123 | 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 | /* If we've received an ACK, then someone beat us to the TAG and
|
| 133 | * we need to wait. */ |
| 134 | state = TOP_HUNTER_WAIT; |
| 135 | } else {
|
| 136 | /* Prepare and send the TAG packet */
|
| 137 | send_buffer[0] = HUNTER_PREY_ACTION_TAG;
|
| 138 | send_buffer[1] = robotID;
|
| 139 | wl_basic_send_global_packet(42, send_buffer, 2); |
| 140 | |
| 141 | /* Record the time so we don't spam a TAG message on the network */
|
| 142 | 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 | /* Check if we've received a new wireless packet */
|
| 153 | |
| 154 | if (packet_data[1] == robotID) { |
| 155 | /* We've been ACKed, so we can now become the prey */
|
| 156 | state = TOP_PREY_AVOID; |
| 157 | prey_state = PREY_START_BACK; |
| 158 | } else {
|
| 159 | /* If we get an ACK with a different robotID, then someone beat us
|
| 160 | * to the TAG, so we must wait */ |
| 161 | state = TOP_HUNTER_WAIT; |
| 162 | } |
| 163 | |
| 164 | } else if (curTime - oldTime > 1) { |
| 165 | /* If 1 second has ellapsed, return to normal hunting state (we can
|
| 166 | * TAG again now) */ |
| 167 | state = TOP_HUNTER_HUNT; |
| 168 | } 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 | oldTime = curTime; |
| 174 | } else {
|
| 175 | /* If no other behavioral changes need to be made, then continue
|
| 176 | * with the hunter FSM where we left off */ |
| 177 | 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 | /* Check if we've received a TAG yet. If so then send an ACK back */
|
| 188 | |
| 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 | /* If we haven't received a TAG yet, continue with prey FSM */
|
| 196 | bom_on(); |
| 197 | prey_state = prey_FSM(prey_state); |
| 198 | } |
| 199 | break;
|
| 200 | case TOP_HUNTER_WAIT: |
| 201 | /* Set orb colors and wait to give the prey the 5 second head start */
|
| 202 | orbs_set_color(BLUE, BLUE); |
| 203 | bom_off(); |
| 204 | motors_off(); |
| 205 | delay_ms(WAIT_DELAY_MS); |
| 206 | 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 | |
| 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 | int prey_FSM(int prey_state) { |
| 239 | |
| 240 | /* Variable to store the front rangefinder readings */
|
| 241 | int rangeVals[3] = {0, 0, 0}; |
| 242 | |
| 243 | 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 | encoder_rst_dx(RIGHT); |
| 250 | return PREY_BACKING;
|
| 251 | break;
|
| 252 | case PREY_BACKING: |
| 253 | if (encoder_get_x(LEFT) < BACK_THRESHOLD
|
| 254 | || encoder_get_x(RIGHT) < BACK_THRESHOLD) {
|
| 255 | motor_l_set(BACKWARD, 255);
|
| 256 | motor_r_set(FORWARD, 255);
|
| 257 | encoder_rst_dx(LEFT); |
| 258 | encoder_rst_dx(RIGHT); |
| 259 | return PREY_TURN;
|
| 260 | } else {
|
| 261 | return PREY_BACKING;
|
| 262 | } |
| 263 | break;
|
| 264 | case PREY_TURN: |
| 265 | if (encoder_get_x(LEFT) < -TURN_DIST
|
| 266 | || encoder_get_x(RIGHT) > TURN_DIST) {
|
| 267 | 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 | /* Drive away if we detect obstacles using the rangefinders */
|
| 278 | if (rangeVals[1] > 0 && rangeVals[1] < IR_DIST_THRESHOLD) { |
| 279 | 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 | 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 | |
| 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 | 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 | |
| 340 | 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 | } |