/* Rug Warrior demo -- Demonstrate some of Rug Warriors capabilities Use the reset button to select one of three modes: 1) Seek light -- Rug Warrior looks for the brightest light 2) Seek darkness -- Rug Warrior tries to hide in the darkest corner 3) Wait for Whistle -- Rug Warrior becomes active when it hears a sound. In mode 1 the robot moves about the room heading toward the brightest spot. While it does this it uses its IR sensors to try to avoid obstacles. If it bumps into an obstacle the collision is detected by the force sensing skirt and the robot takes action to escape the collision. If you shine a flash light at the robot you should be able to get it to follow the beam. Mode 2 is just like mode 1 except that the robot looks for the darkest spot rather than the brightest. In Mode 3 the robot demonstrates its sound detection feature. When it hears a loud sound it plays an alert sound. Bumping the robot from the rear causes it to play Bicycle Built for Two. This only works in mode 1 or 2. The display shows which demo is running on the first line and information from three of the sensors on the second line: IR Bmp Pho If the number following IR is 0 it means that the IR sensor detects no obstacles. A 1 or 2 indicates an object to the left or right. The number following Bmp shows the state of the bumper. Zero means no collision, 1 through 7 indicate (bitwise) which shwitches are closed. The Pho number reports the difference in light level between the left and right photo cells. This demo is intended only as a starting point for more capable robot programs of your own design. If it seems deficient, please improve it. (The demo does not make use of the shaft encoders or the pyro sensor.) */ int T = 1; int NIL = 0; int STOP = 0; int FORWARD = 1; int BACKWARD = 2; int LEFT_TURN = 3; int RIGHT_TURN = 4; int LEFT_ARC = 5; int RIGHT_ARC = 6; int cruise_command = FORWARD; int photo_command = STOP; int ir_command = STOP; int bump_command = STOP; /* Empirical motion-timing data (depends on wheels and motors) */ int msec_per_rev = 2000; /* mSeconds to make 1 revolution */ int msec_per_rad = 340; /* mSeconds to move 1 robot radius */ int rev_2 = msec_per_rev / 2; /* 180 degrees */ int rev_4 = msec_per_rev / 4; /* 90 degrees */ int rev_8 = msec_per_rev / 8; /* 45 degrees */ int back = 0b100; /* Bumper status bits */ int left = 0b010; int right = 0b001; int bump_left = 0; /* Bumper status vars */ int bump_right = 0; int bump_back = 0; int ir_left = 0; int ir_right = 0; int ir_detect_mask = 0b10000; /* Bit E4 */ int ir_mask_l = 0b1000; /* Bit D3 */ int ir_mask_r = 0b0100; /* Bit D2 */ int ir_mask = ir_mask_l | ir_mask_r; int cruise_active = T; int photo_active = NIL; int ir_active = NIL; int bump_active = NIL; int photo_cal = -8; int photo_dead_zone = 3; /* If the photo cells aren't equal... */ int m_mask_l = 0b100000; /* Motor mask bits */ int m_mask_r = 0b010000; int m_mask = m_mask_l | m_mask_r; /* A5,4 => Enable Left, Right. D5,4 => 0 - forward, 1 - backward */ /* int on = 0xff; */ int abs(int arg) { if (arg < 0) return -1 * arg; else return arg; } int limit(int val, int low, int high) /* Limit val to the range low to high */ { if (val < low) return low; else if (val > high) return high; return val; } void wait(int milli_seconds) /* Don't busy wait, check a timer */ { long timer_a; timer_a = mseconds() + (long) milli_seconds; while( timer_a > mseconds() ) { defer(); } } /* Switch uses test_number, a persistent variable defined in lib_rw11.c */ int switch() /* Use the RESET button as a switch */ { if ((test_number < 0) || (test_number > 2)) { test_number = 0; /* Reset test_number */ return 0;} else return test_number; } void move(int operation) { if (operation == STOP) /* 0 - Stop */ drive(0.,0.); else if (operation == FORWARD) /* 1 - Forward */ drive(85.,0.); else if (operation == BACKWARD) /* 2 - Backward */ drive(-85.,0.); else if (operation == LEFT_TURN) /* 3 - Left Turn */ drive(0.,85.); else if (operation == RIGHT_TURN) /* 4 - Right Turn */ drive(0.,-85.); else if (operation == LEFT_ARC) /* 5 - Left Arc turn */ drive(50.,35.); else if (operation == RIGHT_ARC) /* 6 - Right Arc turn */ drive(50.,-35.); } /* The arbitration scheme is as follows: Each behavior may compute an output for the motors. It if does it stores the value in a global and sets an active flag. The master motor controller resets the flags and sends the highest priorty command to the motor. */ void warble(int t_start, int t_stop, int t_del) { int tone_i; beeper_on(); for (tone_i = t_start; (abs(tone_i - t_stop)) > (2 * abs(t_del)); tone_i = tone_i + t_del) {set_beeper_pitch( (float) tone_i); msleep((long)1); } beeper_off(); } void motor_control() { while (1) { if (switch() == 2) /* Stop while listening... */ move(STOP); else if (bump_active) move(bump_command); else if (ir_active) move(ir_command); else if (photo_active) drive(50.0, (float) del_out * p_gain); else if (cruise_active) move(cruise_command); else move(STOP); /* No commands => STOP */ defer(); /* Update once per scheduler iteration */ }} int cruise() /* Default activity */ { while(1) { cruise_command = FORWARD; /* Robot forward */ cruise_active = 1; wait(1000); /* Cruise once a second */ }} int del_out; float p_gain = 2.0; /* Bright light => low numbers */ int photo() /* Follow a light */ { int lpc, rpc, delta; /* Left and Right Photo Cells */ while (1) { lpc = analog(1) + photo_cal; rpc = analog(0); /* delta = rpc - lpc; /* + => left sees bright, - => right */ /* New for demo */ if (switch() == 0) delta = rpc - lpc; /* Seek Light */ else delta = lpc - rpc; /* Seek Darkness */ del_out = delta; /* DEBUGGING */ if ( abs(delta) > photo_dead_zone ) {if (delta > 0) photo_command = LEFT_ARC; /* Left bright => turn left */ else photo_command = RIGHT_ARC; /* Otherwise turn right */ photo_active = 1; /* Activate when detected */ } else photo_active = 0; /* Deactivate when not detected */ defer(); /* Once per scheduler tick */ } } void bump_check() /* Check for current bumps */ { int bump_stat; bump_stat = bumper(); if (left & bump_stat) bump_left = 1; else bump_left = 0; if (right & bump_stat) bump_right = 1; else bump_right = 0; if (back & bump_stat) bump_back = 1; else bump_back = 0; } int recent_hits = 0; /* Keep track of recent collisions */ void bump() { while (1) { bump_check(); if ((bump_left | bump_right) != 0) /* Any collisions? */ recent_hits++; /* Add to count of recent hits */ else recent_hits--; /* Let hits drain away */ recent_hits = limit(recent_hits, 0, 4); /* Limit the range or rh */ if (bump_left && bump_right) { bump_active = 1; bump_command = BACKWARD; wait(msec_per_rad / 2); /* Move back a bit */ bump_command = LEFT_TURN; wait(rev_4); } else if (bump_left) { bump_active = 1; bump_command = RIGHT_TURN; wait(recent_hits * rev_8); } else if (bump_right) { bump_active = 1; bump_command = LEFT_TURN; wait(recent_hits * rev_8); } else if (bump_back) { bump_active = 1; bump_command = LEFT_TURN; wait(recent_hits * rev_4); } else bump_active = 0; defer(); } } /* 0b1000 turns left emitter on, 0b0100 turns right emitter on */ /* ir_detect returns: */ /* 0 => both off, 0b01 => right on, 0b10 => left on, 0b11 => both on */ int ir_out; void ir() { int val; while (1) { /* New for demo */ if (switch() == 2) /* Sound mode */ { leds(0); /* Do it for sound mode */ val = 0; } else /* Otherwise do it the normal way */ val = ir_detect(); sleep(0.25); /* Don't change too often */ ir_out = val; /* DEBUG */ if (val == 0b11) /* Left and right */ { ir_active = T; ir_command = LEFT_ARC; } else if (val == 0b01) /* Left IR */ { ir_active = T; ir_command = RIGHT_ARC; } else if (val == 0b10) /* Right IR */ { ir_active = T; ir_command = LEFT_ARC; } else { ir_active = NIL; } defer(); } } void alert_tune() { tone(1046.5,0.200); tone(1396.9,0.200); tone(1046.5,0.200); tone(698.5,0.200); } int max, min; int cal_sound(int count) { int k,val; k = 0; max = 0; min = 255; tone(1660.,.1); sleep(.3); while (k < count) { k++; val = analog(2); /* Read mic */ if (val > max) max = val; if (val < min) min = val; } tone(880.,.1); return max - min; } void bicycle() /* Bicycle built for two */ { tone(2093.0,0.769); tone(1760.0,0.769); tone(1396.9,0.769); tone(1046.5,0.769); tone(1174.7,0.192); tone(1318.5,0.192); tone(1396.9,0.192); tone(1174.7,0.385); tone(1396.9,0.192); tone(1046.5,0.962); sleep(0.385); tone(1568.0,0.769); tone(2093.0,0.769); tone(1760.0,0.769); tone(1396.9,0.769); tone(1174.7,0.192); tone(1318.5,0.192); tone(1396.9,0.192); tone(1568.0,0.385); tone(1760.0,0.192); tone(1568.0,0.962); sleep(0.385); tone(1760.0,0.192); tone(1864.7,0.192); tone(1760.0,0.192); tone(1568.0,0.192); tone(2093.0,0.385); tone(1760.0,0.192); tone(1568.0,0.192); tone(1396.9,0.962); tone(1568.0,0.192); tone(1760.0,0.385); tone(1396.9,0.192); tone(1174.7,0.385); tone(1396.9,0.192); tone(1174.7,0.192); tone(1046.5,0.962); tone(1046.5,0.192); tone(1396.9,0.385); tone(1760.0,0.192); tone(1568.0,0.385); tone(1046.5,0.192); tone(1396.9,0.385); tone(1760.0,0.192); tone(1568.0,0.192); tone(1760.0,0.192); tone(1864.7,0.192); tone(2093.0,0.192); tone(1760.0,0.192); tone(1396.9,0.192); tone(1568.0,0.385); tone(1046.5,0.192); tone(1396.9,0.962); } void printer() /* Print status info to the screen */ { while (1) /* Line 1 */ { if (switch() == 0) printf("Seek light "); else if (switch() == 1) printf("Seek darkness "); else printf("Wait for whistle"); /* Line 2 */ printf("IR%d ", ir_out); printf("Bmp%d ", bumper()); printf("Pho%d\n", del_out); sleep(0.35); } } int sound_thresh = 220; void check_sound () { while (1) { if (switch() == 2) { if (analog(2) > sound_thresh) {warble(1500,300,-60); warble(1500,300,-60); warble(1500,300,-60);}} else defer(); }} /* Play a tune if bumped from the back */ void check_bump() { while (1) { if (bumper() == 0b100) { if (switch() != 2) bicycle(); } else defer(); } } void main() { sleep(0.75); test_number++; /* Update test number */ alert_tune(); /* Show that processes are starting */ start_process(printer()); /* For debugging */ start_process(motor_control()); start_process(cruise()); start_process(photo()); start_process(ir()); start_process(bump()); start_process(check_sound()); /* Sometimes we listen for a sound */ start_process(check_bump()); }