root / trunk / code / behaviors / hunter_prey / john / main.c @ 1501
History | View | Annotate | Download (9.53 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 | #define WL_CHANNEL 15 |
||
| 18 | |||
| 19 | #define BACK_THRESHOLD -5000 |
||
| 20 | #define TURN_DIST 1024 |
||
| 21 | #define IR_DIST_THRESHOLD 200 |
||
| 22 | |||
| 23 | /* State Macros */
|
||
| 24 | |||
| 25 | 1500 | jsexton | /* Top Level FSM States */
|
| 26 | 1486 | jsexton | #define TOP_INIT 0 |
| 27 | #define TOP_HUNTER_HUNT 1 |
||
| 28 | #define TOP_HUNTER_TAG 2 |
||
| 29 | #define TOP_HUNTER_PURSUE 3 |
||
| 30 | #define TOP_PREY_AVOID 4 |
||
| 31 | #define TOP_HUNTER_WAIT 5 |
||
| 32 | #define TOP_ERROR 6 |
||
| 33 | |||
| 34 | 1500 | jsexton | /* Hunter FSM States */
|
| 35 | 1486 | jsexton | #define HUNTER_SPIRAL 0 |
| 36 | #define HUNTER_CHASE 1 |
||
| 37 | |||
| 38 | 1500 | jsexton | /* Prey FSM States */
|
| 39 | 1486 | jsexton | #define PREY_START_BACK 0 |
| 40 | #define PREY_BACKING 1 |
||
| 41 | #define PREY_TURN 2 |
||
| 42 | #define PREY_AVOID 3 |
||
| 43 | |||
| 44 | |||
| 45 | /* Function prototype declarations */
|
||
| 46 | int hunter_FSM(int, int, int); |
||
| 47 | int prey_FSM(int); |
||
| 48 | |||
| 49 | /* Variables used to receive packets */
|
||
| 50 | unsigned char* packet_data; |
||
| 51 | int data_length;
|
||
| 52 | |||
| 53 | /* Data buffer used to send packets */
|
||
| 54 | char send_buffer[1]; |
||
| 55 | |||
| 56 | int main(void) |
||
| 57 | {
|
||
| 58 | |||
| 59 | /* Initialize dragonfly board */
|
||
| 60 | dragonfly_init(ALL_ON); |
||
| 61 | xbee_init(); |
||
| 62 | encoders_init(); |
||
| 63 | |||
| 64 | /* Initialize the basic wireless library */
|
||
| 65 | wl_basic_init_default(); |
||
| 66 | |||
| 67 | /* Set the XBee channel to assigned channel */
|
||
| 68 | wl_set_channel(WL_CHANNEL); |
||
| 69 | |||
| 70 | |||
| 71 | /* ****** CODE HERE ******* */
|
||
| 72 | |||
| 73 | /* Initialize state machines */
|
||
| 74 | int state = TOP_INIT;
|
||
| 75 | int hunter_state = HUNTER_SPIRAL;
|
||
| 76 | int prey_state = PREY_AVOID;
|
||
| 77 | |||
| 78 | int frontIR = 0; |
||
| 79 | int maxBOM = 0; |
||
| 80 | int robotID = get_robotid();
|
||
| 81 | int oldTime = 0, curTime = 0; |
||
| 82 | |||
| 83 | while (1) { |
||
| 84 | |||
| 85 | 1500 | jsexton | /* Check if we've received a wireless packet */
|
| 86 | 1486 | jsexton | packet_data = wl_basic_do_default(&data_length); |
| 87 | |||
| 88 | 1500 | jsexton | /* Top level state machines */
|
| 89 | 1486 | jsexton | switch(state) {
|
| 90 | |||
| 91 | case TOP_INIT:
|
||
| 92 | orbs_set_color(RED, GREEN); |
||
| 93 | delay_ms(500);
|
||
| 94 | orbs_set_color(GREEN, RED); |
||
| 95 | delay_ms(500);
|
||
| 96 | |||
| 97 | 1500 | jsexton | /* Allow user to pick the starting behavior */
|
| 98 | 1486 | jsexton | if (button1_read()) {
|
| 99 | state = TOP_PREY_AVOID; |
||
| 100 | prey_state = PREY_AVOID; |
||
| 101 | } else {
|
||
| 102 | state = TOP_HUNTER_HUNT; |
||
| 103 | hunter_state = HUNTER_SPIRAL; |
||
| 104 | } |
||
| 105 | break;
|
||
| 106 | case TOP_HUNTER_HUNT:
|
||
| 107 | orbs_set_color(RED, RED); |
||
| 108 | |||
| 109 | if (packet_data && data_length == 2 |
||
| 110 | && packet_data[0] == HUNTER_PREY_ACTION_ACK) {
|
||
| 111 | 1500 | jsexton | /* If we've received an ACK, we need to wait */
|
| 112 | 1486 | jsexton | state = TOP_HUNTER_WAIT; |
| 113 | } else {
|
||
| 114 | 1500 | jsexton | /* Record some sensor readings and check if we can TAG */
|
| 115 | 1486 | jsexton | bom_refresh(BOM_ALL); |
| 116 | frontIR = range_read_distance(IR2); |
||
| 117 | maxBOM = get_max_bom(); |
||
| 118 | if (hunter_prey_tagged(maxBOM, frontIR) || button1_read()) {
|
||
| 119 | state = TOP_HUNTER_TAG; |
||
| 120 | } else {
|
||
| 121 | 1500 | jsexton | /* If we haven't tagged, then enter hunter FSM */
|
| 122 | 1486 | jsexton | hunter_state = hunter_FSM(hunter_state, maxBOM, frontIR); |
| 123 | } |
||
| 124 | } |
||
| 125 | break;
|
||
| 126 | case TOP_HUNTER_TAG:
|
||
| 127 | orbs_set_color(RED, PURPLE); |
||
| 128 | delay_ms(500);
|
||
| 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 | 1486 | jsexton | delay_ms(5000);
|
| 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 | 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 | return PREY_BACKING;
|
||
| 250 | break;
|
||
| 251 | case PREY_BACKING:
|
||
| 252 | if (encoder_get_x(LEFT) < BACK_THRESHOLD) {
|
||
| 253 | motor_l_set(BACKWARD, 255);
|
||
| 254 | motor_r_set(FORWARD, 255);
|
||
| 255 | encoder_rst_dx(RIGHT); |
||
| 256 | return PREY_TURN;
|
||
| 257 | } else {
|
||
| 258 | return PREY_BACKING;
|
||
| 259 | } |
||
| 260 | break;
|
||
| 261 | case PREY_TURN:
|
||
| 262 | if (encoder_get_x(RIGHT) > TURN_DIST) {
|
||
| 263 | return PREY_AVOID;
|
||
| 264 | } else {
|
||
| 265 | return PREY_TURN;
|
||
| 266 | } |
||
| 267 | break;
|
||
| 268 | case PREY_AVOID:
|
||
| 269 | rangeVals[0] = range_read_distance(IR1);
|
||
| 270 | rangeVals[1] = range_read_distance(IR2);
|
||
| 271 | rangeVals[2] = range_read_distance(IR3);
|
||
| 272 | |||
| 273 | 1500 | jsexton | /* Drive away if we detect obstacles using the rangefinders */
|
| 274 | 1486 | jsexton | if (rangeVals[1] > 0 && rangeVals[1] < IR_DIST_THRESHOLD) { |
| 275 | motor_l_set(FORWARD, 255);
|
||
| 276 | motor_r_set(BACKWARD, 255);
|
||
| 277 | return PREY_AVOID;
|
||
| 278 | } else if (rangeVals[0] > 0 && rangeVals[0] < IR_DIST_THRESHOLD) { |
||
| 279 | motor_l_set(FORWARD, 255);
|
||
| 280 | motor_r_set(FORWARD, 170);
|
||
| 281 | return PREY_AVOID;
|
||
| 282 | } else if (rangeVals[2] > 0 && rangeVals[2] < IR_DIST_THRESHOLD) { |
||
| 283 | motor_l_set(FORWARD, 170);
|
||
| 284 | motor_r_set(FORWARD, 255);
|
||
| 285 | return PREY_AVOID;
|
||
| 286 | } else {
|
||
| 287 | motor_l_set(FORWARD, 255);
|
||
| 288 | motor_r_set(FORWARD, 255);
|
||
| 289 | return PREY_AVOID;
|
||
| 290 | } |
||
| 291 | break;
|
||
| 292 | default:
|
||
| 293 | return PREY_AVOID;
|
||
| 294 | break;
|
||
| 295 | |||
| 296 | } |
||
| 297 | |||
| 298 | return prey_state;
|
||
| 299 | |||
| 300 | } |
||
| 301 | |||
| 302 | 1500 | jsexton | |
| 303 | /*
|
||
| 304 | * hunter_FSM - Hunter finite state machine which defaults to spiraling
|
||
| 305 | * outwards until the BOM can locate the prey. Once the BOM locates
|
||
| 306 | * the prey, chase the prey as fast as possible.
|
||
| 307 | *
|
||
| 308 | * Arguments:
|
||
| 309 | * hunter_state - Current hunter state.
|
||
| 310 | * maxBOM - Current maximum BOM value.
|
||
| 311 | * frontIR - Current front IR rangefinder reading value.
|
||
| 312 | *
|
||
| 313 | * returns - The new state of the hunter state machine.
|
||
| 314 | */
|
||
| 315 | |||
| 316 | 1486 | jsexton | int hunter_FSM(int hunter_state, int maxBOM, int frontIR) { |
| 317 | |||
| 318 | switch(hunter_state) {
|
||
| 319 | |||
| 320 | case HUNTER_SPIRAL:
|
||
| 321 | if (maxBOM != -1) { |
||
| 322 | return HUNTER_CHASE;
|
||
| 323 | } else {
|
||
| 324 | motor_l_set(FORWARD, 170);
|
||
| 325 | motor_r_set(FORWARD, 190);
|
||
| 326 | return HUNTER_SPIRAL;
|
||
| 327 | } |
||
| 328 | break;
|
||
| 329 | case HUNTER_CHASE:
|
||
| 330 | 1500 | jsexton | |
| 331 | /* A large look-up table to set the speed of the motors based on
|
||
| 332 | * where the prey robot is. */
|
||
| 333 | 1486 | jsexton | if (maxBOM == -1) { |
| 334 | return HUNTER_CHASE;
|
||
| 335 | } else if (maxBOM == 4) { |
||
| 336 | motor_l_set(FORWARD, 255);
|
||
| 337 | motor_r_set(FORWARD, 255);
|
||
| 338 | return HUNTER_CHASE;
|
||
| 339 | } else if (maxBOM == 3) { |
||
| 340 | motor_l_set(FORWARD, 255);
|
||
| 341 | motor_r_set(FORWARD, 240);
|
||
| 342 | return HUNTER_CHASE;
|
||
| 343 | } else if (maxBOM == 5) { |
||
| 344 | motor_l_set(FORWARD, 240);
|
||
| 345 | motor_r_set(FORWARD, 255);
|
||
| 346 | return HUNTER_CHASE;
|
||
| 347 | } else if (maxBOM < 3) { |
||
| 348 | motor_l_set(FORWARD, 255);
|
||
| 349 | motor_r_set(FORWARD, 170);
|
||
| 350 | return HUNTER_CHASE;
|
||
| 351 | } else if (maxBOM > 5 && maxBOM <= 8) { |
||
| 352 | motor_l_set(FORWARD, 170);
|
||
| 353 | motor_r_set(FORWARD, 255);
|
||
| 354 | return HUNTER_CHASE;
|
||
| 355 | } else if (maxBOM > 8 && maxBOM < 12) { |
||
| 356 | motor_l_set(BACKWARD, 255);
|
||
| 357 | motor_r_set(FORWARD, 255);
|
||
| 358 | return HUNTER_CHASE;
|
||
| 359 | } else {
|
||
| 360 | motor_l_set(FORWARD, 255);
|
||
| 361 | motor_r_set(BACKWARD, 255);
|
||
| 362 | return HUNTER_CHASE;
|
||
| 363 | } |
||
| 364 | break;
|
||
| 365 | default:
|
||
| 366 | return HUNTER_SPIRAL;
|
||
| 367 | break;
|
||
| 368 | |||
| 369 | } |
||
| 370 | |||
| 371 | return hunter_state;
|
||
| 372 | |||
| 373 | } |