// Heartbeat and state machine demo for CSC 230 Homebot project // This code implements a behavioral abstraction for robot control // based on a finite-state machine model. // Basically, the robot is considered, at any time, to be executing some behavior // (e.g. stopped, driving forward) that is represented by a state. // Various conditions can trigger transitions between behaviors // (e.g. time elapsed, switch press, or other sensor condition) // The behavior and transition conditions are implemented in state implementation routines // with the generic forms start_XXX() and resume_XXX(). The main heartbeat loop branches to the // appropriate state implementation routine once every heartbeat. // This program architecture permits a flexible co-routine-like programming model // where display, sensor read, and behavioral commands all execute "simultaneously" // (abstractly) while maintaining a constant microcontroller heartbeat, here running at 100 Hz. // Of course nothing is really going on simultaneously, it's effectively fine-grained time-sharing. // This demo code contains implementations for low-level motion-behaviors stop, drive forward and reverse, and // rotate clockwise and counterclockwise. It also contains three "higher-level" behaviors "rest", "dance", and "explore" // illustrating one way that higher-level behaviors can be build from lower-level ones. // It is by no means the only way of accomplishing that goal. // This code is illustrative only , and not intended to be exactly what you put on your bot. // In fact, it will almost certainly need to be modified to work on what you have built. // However, you are welcome to incorporate whatever pieces you find useful into your own control code. // Pin definitions for Arduino Uno //LED pins #define INT_LED 13 #define RED_LED 2 #define YELLOW_LED 3 #define GREEN_LED 4 #define BLUE_LED 5 // Pin for piezo speaker #define SPEAKER_PIN 8 // Audio modes #define NO_AUDIO 0 #define CRICKET 10 int Audio = NO_AUDIO; // Homebot has two continuous-rotation servos. The set point controls speed rather than position. // A PWM pulse width of 1500 us is zero speed. // The maximum velocity is about 60rpm clockwise (2100 us) and counterclockwise (900 us) // Servo pins #define RIGHT_WHEEL_PIN 10 // use 9 and 10 if you want to use tone() for sound #define LEFT_WHEEL_PIN 9 #include Servo right_wheel, left_wheel; #define CENTER_SET 1500 // 0 speed #define MAX_SET 2100 // about 60 rpm clockwise #define MIN_SET 900 // about 60 rpm counterclockwise #define MAX_SPEED 600 // +- variation in us about CENTER_SET #define HALF_SPEED 300 #define QUARTER_SPEED 150 int bot_speed = 0; // 0 to 600 // The "set" variables are "shadow variables" to indicate where the system believes // the servo speeds to be set since there is no read-back from the servos themselves. unsigned int right_wheel_set = CENTER_SET; unsigned int left_wheel_set = CENTER_SET; // Whisker switch sensor pins #define RIGHT_WHISKER_PIN 7 #define LEFT_WHISKER_PIN 6 int cur_right_whisker = LOW; int cur_left_whisker = LOW; int prev_right_whisker = LOW; int prev_left_whisker = LOW; int right_whisker_stable = 0; // heartbeats that whisker value has been stable int left_whisker_stable = 0; // Infrared distance sensors. // With 6V alkaline AA battery-pack input, output voltage ranges approximately from .5 to 3 volts, // and in that range displays an inverse relationship to distance. // The max occurs near the specified // sensor minimum range, and falls quickly and unreliably for closer distances // For targets beyond the specified sensor maximum, the output voltage is low, but unstable. // Individal sensors produce repeatable measurements, but there is between-sensor variation in outputs. // The long-range sensor on the Prof's protoype bot produced the following outputs: // .4V = 30", .5V = 22", .6V = 17", .75V = 12" 1.0V = 10", 2.0V = 5", 2.4V = 4", 2.9V = 3" // For the short-range sensor, mounted at about 2" up and pointed own at 45 degrees, // less than 1 volt is a good dropoff detection voltage. #define DOWN_IR_PIN A1 // short-range 4-30cm sensor #define FORWARD_IR_PIN A2 // long-range 20-150cm sensor #define DOWN_IR_THRESH 200 // Drop-off detection - about 1 Volt. V < thresh => dropoff #define FORWARD_IR_THRESH 490 // Collision thresh, about 2.4 Volts. V > Thresh => too close int down_ir_val = 400; // An OK, non-dropoff value int forward_ir_val = 200; // OK distance // States. What is the bot is doing. // Associated with each state is a set of globally accessible control parameters // that permit the behavior to be modified, generally on initial invokation. // *_count is number of heartbeats state has been active (since start of reset) // *_timeout in heartbeats is used to specify a (maximum) time in the state. // *_speed is used to set speed of motion // *_return_state specifies the state to return to if behavior was invoked in a return context. // ST_NULL indicates no return. // In a return situation, the state returns to the state which invoked the transition with no modification // of that state's parameters - i.e. it picks up where it left off #define ST_NULL 0 // Used when there is no relevant state // Low-level motion states - mutually exclusive set #define ST_STOP 10 // Base resting state. #define ST_MOVE_GENERAL 20 #define ST_DRIVE_FWD 21 #define ST_DRIVE_GUARDED 22 #define ST_DRIVE_RVS 23 #define ST_ROTATE_CLOCK 24 #define ST_ROTATE_COUNTER 25 int Motion_state = ST_STOP; unsigned long Motion_state_count = 0; // Cycles system has been in current motion state // Mid-level activity states - mutually exclusive set #define ST_REST 300 #define ST_DANCE 400 #define ST_EXPLORE 500 #define ST_SING 600 int Activity_state = ST_REST; unsigned long Activity_state_count = 0; // Heartbeat timing #define SECONDS_1 100 #define SECONDS_2 200 #define SECONDS_5 500 #define SECONDS_10 1000 #define FOREVER 1000000ul // A million cycles - about a day, practically forever... #define HEARTBEAT_USEC 10000ul // 10 milliseconds = 100Hz unsigned long loop_start_usec; unsigned long work_done_usec; unsigned long usec_used; unsigned long delay_usec; unsigned long delay_ms; unsigned int remainder_usec; unsigned long Count = 0; unsigned int Count2 = 0; unsigned int Count4 = 0; unsigned int Count5 = 0; unsigned int Count10 = 0; unsigned int Count100 = 0; // One second at 100 Hz heartbeat unsigned int Count200 = 0; unsigned int Count1600 = 0; // 16 second counter //************************************************************************************** // setup() runs once when you power up the board or press reset void setup() { pinMode(INT_LED, OUTPUT); pinMode(RED_LED, OUTPUT); pinMode(YELLOW_LED, OUTPUT); pinMode(GREEN_LED, OUTPUT); pinMode(BLUE_LED, OUTPUT); // Make sure all the LEDs are off initially digitalWrite(RED_LED, LOW); digitalWrite(YELLOW_LED, LOW); digitalWrite(GREEN_LED, LOW); digitalWrite(BLUE_LED, LOW); pinMode(RIGHT_WHEEL_PIN, OUTPUT); pinMode(LEFT_WHEEL_PIN, OUTPUT); pinMode(RIGHT_WHISKER_PIN, INPUT); pinMode(LEFT_WHISKER_PIN, INPUT); pinMode(DOWN_IR_PIN, INPUT); pinMode(FORWARD_IR_PIN, INPUT); } //*************************************************************************************** // loop() runs repeatedly. If program terminates or runs off the end, loop() starts over. void loop() { // LED sequence on startup digitalWrite(RED_LED, HIGH); delay(1000); digitalWrite(YELLOW_LED, HIGH); delay(1000); digitalWrite(GREEN_LED, HIGH); delay(1000); digitalWrite(BLUE_LED, HIGH); delay(1000); // Play the little baseball fanfare... tone(SPEAKER_PIN, 262, 200); // middle C C4) for .2 second delay(200); tone(SPEAKER_PIN, 349, 200); // F4 delay(200); tone(SPEAKER_PIN, 440, 200); // A5 delay(200); tone(SPEAKER_PIN, 523, 300); // C5 + phrasing break delay(400); tone(SPEAKER_PIN, 440, 200); // A5 delay(200); tone(SPEAKER_PIN, 523, 600); // C5 delay(600); digitalWrite(RED_LED, LOW); digitalWrite(YELLOW_LED, LOW); digitalWrite(GREEN_LED, LOW); digitalWrite(BLUE_LED, LOW); delay(1000); // Set servo speeds to 0 before attaching // Otherwise, odd behavior can result on startup // and sometimes does anyway... set_servos(CENTER_SET, CENTER_SET); delay(50); // Attach servos right_wheel.attach(RIGHT_WHEEL_PIN, 900, 2100); // Range of HiTech HSR-2648CR continuous rotationservo delay(50); left_wheel.attach(LEFT_WHEEL_PIN, 900, 2100); delay(50); bot_speed = HALF_SPEED; // 0 - 600. Half max speed, just for testing // Initialize various count and state variables Count = 0; Count2 = 0; Count4 = 0; Count5 = 0; Count10 = 0; Count100 = 0; Count200 = 0; Count1600 = 0; Motion_state = ST_STOP; Motion_state_count = 0; Activity_state = ST_REST; Activity_state_count = 0; Audio = NO_AUDIO; //Audio = CRICKET; start_stop(FOREVER); // Initial motion state start_rest(); // Initial activity state while(true) // Start local, infinite, heartbeat loop running at about 100 Hz { loop_start_usec = micros(); // "housekeeping" funtions that are performed every heartbeat. // Read the whisker switch sensors read_whiskers(); // Read the IR distance sensors read_ir_sensors(); // Branch according to current motion state if(Motion_state == ST_STOP) resume_stop(); else if(Motion_state == ST_DRIVE_FWD) resume_drive_fwd(); else if(Motion_state == ST_DRIVE_GUARDED) resume_drive_guarded(); else if(Motion_state == ST_DRIVE_RVS) resume_drive_rvs(); else if(Motion_state == ST_ROTATE_CLOCK) resume_rotate_clock(); else if(Motion_state == ST_ROTATE_COUNTER) resume_rotate_counter(); // Recovery if system somehow got into an unknown motion state... else Motion_state = ST_STOP; if(Activity_state == ST_REST) resume_rest(); else if(Activity_state == ST_DANCE) resume_dance(); else if(Activity_state == ST_EXPLORE) resume_explore(); // Set the LED display to show what has just happened led_display(); // Sound effects play_audio(); // Wait until time to start next heartbeat cycle. // This keeps system on an accurate schedule, even if time is spent on computations. // Some complexities because the internal us counter loops in an amount of time the robot might be active. work_done_usec = micros(); // usec counter loops after ~70 min if(work_done_usec > loop_start_usec) usec_used = work_done_usec - loop_start_usec; else usec_used = (0xFFFFFFFFul - loop_start_usec) + work_done_usec; if(usec_used >= HEARTBEAT_USEC) delay_usec = 0; else delay_usec = (HEARTBEAT_USEC - usec_used); if(delay_usec < 15000) delayMicroseconds((unsigned int)delay_usec); else { delay_ms = delay_usec/1000; remainder_usec = (unsigned int)(delay_usec - (delay_ms * 1000)); delay(delay_ms); // because delayMicroseconds does not work for values > 16383 delayMicroseconds(remainder_usec); } // Update various loop counters Count++; Count2++; if(Count2 >= 2) Count2 = 0; Count4++; if(Count4 >= 4) Count4 = 0; Count5++; if(Count5 >= 5) Count5 = 0; Count10++; if(Count10 >= 10) Count10 = 0; Count100++; if(Count100 >= 100) Count100 = 0; Count200++; if(Count200 >= 200) Count200 = 0; Count1600++; if(Count1600 >= 1600) Count1600 = 0; } // End local infinite loop } // End loop() function //--------------------------------------------------------------------------------------------- //********************************************************************************************* //--------------------------------------------------------------------------------------------- void set_servos( unsigned int right_wheel_us, unsigned int left_wheel_us) // Command continuous rotation servos to move at specified speeds subject to upper and lower bounds. // All control of wheels should take place through this command. // Modifies global variables *_wheel_set { // Check commands against bounds if(right_wheel_us > MAX_SET) right_wheel_us = MAX_SET; if(right_wheel_us < MIN_SET) right_wheel_us = MIN_SET; if(left_wheel_us > MAX_SET) left_wheel_us = MAX_SET; if(left_wheel_us < MIN_SET) left_wheel_us = MIN_SET; // Send commands to the servos right_wheel.writeMicroseconds(right_wheel_us); left_wheel.writeMicroseconds(left_wheel_us); // Update the shadow variables right_wheel_set = right_wheel_us; left_wheel_set = left_wheel_us; } //--------------------------------------------------------------------------------------------- void read_whiskers() // Read the whisker switches and update the history variables // Called every heartbeat { prev_right_whisker = cur_right_whisker; prev_left_whisker = cur_left_whisker; cur_right_whisker = digitalRead(RIGHT_WHISKER_PIN); cur_left_whisker = digitalRead(LEFT_WHISKER_PIN); if(cur_right_whisker == prev_right_whisker) right_whisker_stable++; else right_whisker_stable = 0; if(cur_left_whisker == prev_left_whisker) left_whisker_stable++; else left_whisker_stable = 0; } void read_ir_sensors() // read the infrared distance sensors { down_ir_val = analogRead(DOWN_IR_PIN); forward_ir_val = analogRead(FORWARD_IR_PIN); // Future: Do some processing on forward ir history for movement detection // (presumeably interesting only when bot is stationary) return; } //--------------------------------------------------------------------------------------------- void led_display() // Flash LEDs to indicate various state information // Called every heartbeat cycle { // Start by turning everything off. // Some will be turned back on almost immediately with probably imperceptible flicker. digitalWrite(RED_LED, LOW); digitalWrite(YELLOW_LED, LOW); digitalWrite(GREEN_LED, LOW); digitalWrite(BLUE_LED, LOW); // Flash brief yellow to indicate stop mode if(Motion_state == ST_STOP) { if(Count100 < 10) digitalWrite(YELLOW_LED, HIGH); } // And brief green to indicate rest activity if(Activity_state == ST_REST) { if(Count100 < 10) digitalWrite(GREEN_LED, HIGH); } // Blink green to indicate drive_fwd mode if(Motion_state == ST_DRIVE_FWD) { if(Count100 < 50) digitalWrite(GREEN_LED, HIGH); } // Blink green twice fast to indicate drive_rvs mode if(Motion_state == ST_DRIVE_RVS) { if(Count100 < 10) digitalWrite(GREEN_LED, HIGH); if(Count100 >= 25 && Count100 < 35) digitalWrite(GREEN_LED, HIGH); } // Blink G-Y-G fast to indicate drive_guarded mode if(Motion_state == ST_DRIVE_GUARDED) { if(Count100 < 10) digitalWrite(GREEN_LED, HIGH); if(Count100 >= 20 && Count100 < 30) digitalWrite(YELLOW_LED, HIGH); if(Count100 >= 40 && Count100 < 50) digitalWrite(GREEN_LED, HIGH); } // Blink blue to indicate rotate clockwise mode if(Motion_state == ST_ROTATE_CLOCK) { if(Count100 < 50) digitalWrite(BLUE_LED, HIGH); } // Blink blue twice fast to indicate rotate counterclockwise mode if(Motion_state == ST_ROTATE_COUNTER) { if(Count100 < 10) digitalWrite(BLUE_LED, HIGH); if(Count100 >= 25 && Count100 < 35) digitalWrite(BLUE_LED, HIGH); } /* // Blink green and blue simultaneously to indicate explore mode if(Activity_state == ST_EXPLORE) { if(Count100 < 50) digitalWrite(GREEN_LED, HIGH); if(Count100 < 50) digitalWrite(BLUE_LED, HIGH); } */ // Whisker signals - just to check they are working if(cur_right_whisker == HIGH) digitalWrite(BLUE_LED, HIGH); if(cur_left_whisker == HIGH) digitalWrite(GREEN_LED, HIGH); // Heartbeat: Flash red once per second, on 500ms, off 500ms. if(Count100 < 50) digitalWrite(RED_LED, HIGH); } void play_audio() { // All we got for now is cricket chirp if(Audio == CRICKET) { // On even half seconds we play F7 (3-1/2 octaves above middle C) // in .03s pulses at 25 Hz if(Count100 < 50) // even half seconds { if(Count4 == 0) tone(SPEAKER_PIN, 2794, 30); } } } //--------------------------------------------------------------------------------------------- //--------------------------------------------------------------------------------------------- //--------------------------------------------------------------------------------------------- // Routines implementing details of various behaviors // The STOP behavior //static state variables unsigned long stop_count = 0; unsigned long stop_timeout = 10000; // parameterized initialization routine void start_stop(unsigned long timeout) { Motion_state = ST_STOP; Motion_state_count = 0; stop_count = 0; stop_timeout = timeout; return; } void resume_stop() // In the stop state, both wheel speeds are set to 0 // At the moment, it is used primarily to signal completion of some motion behaviors // The only way out is for a higher-level activity to invoke another motion. { if(Motion_state != ST_STOP) return; // If somehow we got here by mistake... Motion_state_count++; // increment count of heartbeats we have been in the current state stop_count++; // On even heartbeats, set speed of both wheels to zero if( Count2 == 0) { set_servos(CENTER_SET, CENTER_SET); } // Wait a second before becoming sensitive to various inputs if(Motion_state_count < 100) return; // Possible future timeout exit condition? - boredom? // But what does it even mean to stop stopping? For now, nothing if(stop_count >= stop_timeout) { return; } // Otherwise, we just stay in stop return; } //--------------------------------------------------------------------------------------------- // Drive forward behavior // Static state variables unsigned long drive_fwd_count = 0; unsigned long drive_fwd_pause = 100; // 1 second unsigned long drive_fwd_duration = 10000; // 100 second - very long unsigned long drive_fwd_timeout = 10100; // sum of pause and duration int drive_fwd_speed = HALF_SPEED; // 0 to 600 // parameterized initialization routine void start_drive_fwd(int drive_speed, unsigned long pause, unsigned long duration) { Motion_state = ST_DRIVE_FWD; Motion_state_count = 0; drive_fwd_count = 0; drive_fwd_speed = drive_speed; drive_fwd_pause = pause; drive_fwd_duration = duration; drive_fwd_timeout = pause + duration; return; } void resume_drive_fwd() // In the drive_fwd state, both wheel speeds are set to bot_speed // Accesses global variables Motion_state, and Motion_state_count { if(Motion_state != ST_DRIVE_FWD) return; // If somehow we got here by mistake... Motion_state_count++; // increment count of heartbeats we have been in the current state drive_fwd_count++; // Don't move during initial pause interval if(drive_fwd_count < drive_fwd_pause) { if( Count2 == 0) set_servos(CENTER_SET, CENTER_SET); // make sure the motors stay stopped return; } // On even heartbeats, set speed of both wheels to current assigned speed // Because of mechanical orientation, one wheel must go CW and the other CCW for straight motion if( Count2 == 0) { set_servos(CENTER_SET + drive_fwd_speed, CENTER_SET - drive_fwd_speed); } // Timeout exit condition if(drive_fwd_count >= drive_fwd_timeout) { // go to the stop state start_stop(FOREVER); return; } // Otherwise we stay in drive_fwd return; } //--------------------------------------------------------------------------------------------- // Version of drive-forward that stops when whiskers or drop-off IR detector // are triggered // Static state variables unsigned long drive_guarded_count = 0; unsigned long drive_guarded_pause = 100; // 1 second unsigned long drive_guarded_duration = 10000; // 100 second - very long unsigned long drive_guarded_timeout = 10100; int drive_guarded_speed = HALF_SPEED; // 0 to 600 // Possible reasons for halt #define NO_HALT 0 #define HALT_RIGHT_WHISKER 10 #define HALT_LEFT_WHISKER 11 #define HALT_DROPOFF 20 #define HALT_TIMEOUT 30 int drive_halt_reason = NO_HALT; // // parameterized initialization routine void start_drive_guarded(int drive_speed, unsigned long pause, unsigned long duration) { Motion_state = ST_DRIVE_GUARDED; Motion_state_count = 0; drive_guarded_count = 0; drive_guarded_speed = drive_speed; drive_guarded_pause = pause; drive_guarded_duration = duration; drive_guarded_timeout = pause + duration; drive_halt_reason = NO_HALT; return; } // returns current value (last value set) of halt_reason int get_drive_halt_reason() { return(drive_halt_reason); } void resume_drive_guarded() // In the drive_guarded state, the bot moves forward until a stop condition is raised { if(Motion_state != ST_DRIVE_GUARDED) return; // If somehow we got here by mistake... Motion_state_count++; // increment count of heartbeats we have been in the current state drive_guarded_count++; // Don't move during initial pause if(drive_guarded_count < drive_guarded_pause) { if( Count2 == 0) set_servos(CENTER_SET, CENTER_SET); // make sure the motors stay stopped return; } // Check various sensors for halt conditions and go to stop if triggered if(cur_left_whisker == HIGH) { // Set return value and go to the stop state drive_halt_reason = HALT_LEFT_WHISKER; start_stop(FOREVER); return; } if(cur_right_whisker == HIGH) { drive_halt_reason = HALT_RIGHT_WHISKER; start_stop(FOREVER); return; } // check down-pointing IR sensor for dropoff if(down_ir_val < DOWN_IR_THRESH) { drive_halt_reason = HALT_DROPOFF; start_stop(FOREVER); return; } // Timeout exit condition if(drive_guarded_count >= drive_guarded_timeout) { drive_halt_reason = HALT_TIMEOUT; start_stop(FOREVER); return; } // On even heartbeats, set speed of both wheels to current assigned speed // Because of mechanical orientation, one wheel must go CW and the other CCW for straight motion if( Count2 == 0) { set_servos(CENTER_SET + drive_guarded_speed, CENTER_SET - drive_guarded_speed); } // Otherwise we stay in drive_guarded return; } //--------------------------------------------------------------------------------------------- // Drive in reverse (backup) behavior // static state variables unsigned long drive_rvs_count = 0; unsigned long drive_rvs_pause = 100; // 1 second unsigned long drive_rvs_duration = 10000; // 100 second - very long unsigned long drive_rvs_timeout = 10100; // sum of pause and duration int drive_rvs_speed = HALF_SPEED; // 0 to 600 // parameterized initialization routine void start_drive_rvs(int drive_speed, unsigned long pause, unsigned long duration) { Motion_state = ST_DRIVE_RVS; Motion_state_count = 0; drive_rvs_count = 0; drive_rvs_speed = drive_speed; drive_rvs_pause = pause; drive_rvs_duration = duration; drive_rvs_timeout = pause + duration; return; } void resume_drive_rvs() // In the reverse state, both wheel speeds are set to - bot_speed // Accesses global variables Motion_state, and Motion_state_count { if(Motion_state != ST_DRIVE_RVS) return; // If somehow we got here by mistake... Motion_state_count++; // increment count of heartbeats we have been in the current state drive_rvs_count++; // Don't move for pause interval if(drive_rvs_count < drive_rvs_pause) { if( Count2 == 0) set_servos(CENTER_SET, CENTER_SET); // make sure the motors stay stopped return; } // On even heartbeats, set speed of both wheels to current assigned speed // Because of mechanical orientation, one wheel must go CW and the other CCW for straight motion if( Count2 == 0) { set_servos(CENTER_SET - drive_rvs_speed, CENTER_SET + drive_rvs_speed); } // Timeout exit condition if(drive_rvs_count >= drive_rvs_timeout) { start_stop(FOREVER); return; } // Otherwise we stay in drive_rvs return; } //--------------------------------------------------------------------------------------------- // Clockwise rotation behavior // Static state variables unsigned long rotate_clock_count = 0; unsigned long rotate_clock_pause = 100; // 1 second unsigned long rotate_clock_duration = 10000; // 100 second - very long unsigned long rotate_clock_timeout = 10100; // sum of pause and duration int rotate_clock_speed = HALF_SPEED; // 0 to 600 // parameterized initialization routine void start_rotate_clock(int rotate_speed, unsigned long pause, unsigned long duration) { Motion_state = ST_ROTATE_CLOCK; Motion_state_count = 0; rotate_clock_count = 0; rotate_clock_speed = rotate_speed; rotate_clock_pause = pause; rotate_clock_duration = duration; rotate_clock_timeout = pause + duration; return; } void resume_rotate_clock() // In the rotate clock(wise) state, both wheel speeds are set to rotate counter-clockwise // at bot_speed. 2 seconds is just over 180 degrees at speed = 300. // Accesses global variables Motion_state, and Motion_state_count { if(Motion_state != ST_ROTATE_CLOCK) return; // If somehow we got here by mistake... Motion_state_count++; // increment count of heartbeats we have been in the current state rotate_clock_count++; // Don't move during initial pause interval if(rotate_clock_count < rotate_clock_pause) { if( Count2 == 0) set_servos(CENTER_SET, CENTER_SET); // make sure the motors stay stopped return; } // On even heartbeats, set speed of both wheels to current assigned speed. // Because of mechanical orientation, both wheels must go CCW for clockwise rotation of bot if( Count2 == 0) { set_servos(CENTER_SET - rotate_clock_speed, CENTER_SET - rotate_clock_speed); } // Timeout exit condition if(rotate_clock_count >= rotate_clock_timeout) { start_stop(FOREVER); return; } // Otherwise we stay in rotate_clock return; } //--------------------------------------------------------------------------------------------- // Counterclockwise rotation behavior // Static state variables unsigned long rotate_counter_count = 0; unsigned long rotate_counter_pause = 100; // 1 second unsigned long rotate_counter_duration = 10000; // 100 second - very long unsigned long rotate_counter_timeout = 10100; // sum of pause and duration int rotate_counter_speed = HALF_SPEED; // 0 to 600 // parameterized initialization routine void start_rotate_counter(int rotate_speed, unsigned long pause, unsigned long duration) { Motion_state = ST_ROTATE_COUNTER; Motion_state_count = 0; rotate_counter_count = 0; rotate_counter_speed = rotate_speed; rotate_counter_pause = pause; rotate_counter_duration = duration; rotate_counter_timeout = pause + duration; return; } void resume_rotate_counter() // In the rotate counter(clockwise) state, both wheel speeds are set to rotate clockwise // at bot_speed // Accesses global variables Motion_state, and Motion_state_count { if(Motion_state != ST_ROTATE_COUNTER) return; // If somehow we got here by mistake... Motion_state_count++; // increment count of heartbeats we have been in the current state rotate_counter_count++; // Don't move for initial pause interval if(rotate_counter_count < rotate_counter_pause) { if( Count2 == 0) set_servos(CENTER_SET, CENTER_SET); // make sure the motors stay stopped return; } // On even heartbeats, set speed of both wheels to current assigned speed. // Because of mechanical orientation, both wheels must go CW for counterclockwise rotation of bot if( Count2 == 0) { set_servos(CENTER_SET + rotate_counter_speed, CENTER_SET + rotate_counter_speed); } // Timeout exit condition if(rotate_counter_count >= rotate_counter_timeout) { start_stop(FOREVER); return; } // otherwise we stay in rotate_counter return; } //--------------------------------------------------------------------------------------------- //--------------------------------------------------------------------------------------------- //--------------------------------------------------------------------------------------------- // More complex "Activities" //--------------------------------------------------------------------------------------------- // "Rest" // Initial activity state where bot is sensitive to user "commands" via sensors // e.g. the whisker switches. An "activity" because we need the stop motion state to be simpler. //static state variables unsigned long rest_count = 0; // parameterized initialization routine void start_rest() { // set motion state to stop to eliminate any possible inconsistency start_stop(FOREVER); Activity_state = ST_REST; Activity_state_count = 0; rest_count = 0; //Audio = CRICKET; // Just for fun... return; } void resume_rest() // In the rest state, both wheel speeds are set to 0 { if(Activity_state != ST_REST) return; // If somehow we got here by mistake... Activity_state_count++; // increment count of heartbeats we have been in the current state rest_count++; // On even heartbeats, set speed of both wheels to zero // Logically unecessary because the motion state should be stop, but just in case... if( Count2 == 0) { set_servos(CENTER_SET, CENTER_SET); } // Wait a second before becoming sensitive to various inputs if(rest_count < 100) return; // Leave rest if whiskers are pressed - user input if(cur_left_whisker == HIGH) { Audio = NO_AUDIO; start_dance(); return; } if(cur_right_whisker == HIGH) { Audio = NO_AUDIO; start_explore(); return; } // Future timeout exit condition? Boredom? For now, nothing. // If no (user) signal, just stay resting return; } //--------------------------------------------------------------------------------------------- // "Dance" // A "complex" behavior built on lower-level ones. // Substates #define DANCE_START 401 #define DANCE_FWD 402 #define DANCE_RVS 403 #define DANCE_CLOCK 404 #define DANCE_COUNTER 405 #define DANCE_EXIT 406 // Static state variables unsigned long dance_count = 0; int dance_substate = DANCE_START; // parameterized initialization routine void start_dance() { Activity_state = ST_DANCE; Activity_state_count = 0; dance_count = 0; dance_substate = DANCE_START; return; } void resume_dance() { if(Activity_state != ST_DANCE) return; // If somehow we got here by mistake Activity_state_count++; dance_count++; // First action is to drive forward for 300 heartbeats (1 s pause + 2 s driving) if(dance_substate == DANCE_START) { dance_substate = DANCE_FWD; start_drive_fwd(HALF_SPEED, 100, 200); return; } if(dance_substate == DANCE_FWD) { // Wait until the drive forward behavior has completed, and go to the reverse state if(Motion_state == ST_STOP) { dance_substate = DANCE_RVS; start_drive_rvs(HALF_SPEED, 100, 200); } return; } if(dance_substate == DANCE_RVS) { if(Motion_state == ST_STOP) { dance_substate = DANCE_CLOCK; start_rotate_clock(HALF_SPEED, 100, 200); } return; } if(dance_substate == DANCE_CLOCK) { if(Motion_state == ST_STOP) { dance_substate = DANCE_COUNTER; start_rotate_counter(HALF_SPEED, 100, 200); } return; } if(dance_substate == DANCE_COUNTER) { // Exit condition - go back to rest if(Motion_state == ST_STOP) { Activity_state = ST_REST; start_rest(); } return; } // If somehow there is a screwup and we are in an unrecognized substate, terminate dance Activity_state = ST_REST; start_rest(); return; } //--------------------------------------------------------------------------------------------- // Explore behavior // Another "complex" behavior // Substates #define EXPL_START 501 #define EXPL_DRIVE_FWD 502 #define EXPL_DRIVE_RVS 503 #define EXPL_ROTATE_CLOCK 504 #define EXPL_ROTATE_COUNTER 505 #define EXPL_EXIT 506 unsigned long expl_count = 0; unsigned long expl_substate_count; int expl_substate = EXPL_START; int expl_collision_count = 0; // Reason for halt during drive-forward phase int expl_halt_reason; // // parameterized initialization routine void start_explore() { Activity_state = ST_EXPLORE; Activity_state_count = 0; expl_count = 0; expl_substate = EXPL_START; expl_substate_count = 0; expl_collision_count = 0; expl_halt_reason = NO_HALT; return; } void resume_explore() // Explore is an elementary "complex" behavior. // Bot drives forward until it hits an obstacle triggering a whisker sensor. // When this occurs, it stops, backs up, makes a 90 degree turn away from the triggered whisker // (the direction that would put it on a course away from the triggering flat surface) // and drives forward again. // Requires use of persistent internal state to allow co-routine-like restart between every heartbeat. { if(Activity_state != ST_EXPLORE) return; // If somehow we got here by mistake... Activity_state_count++; // increment count of heartbeats we have been in the explore state expl_count++; expl_substate_count++; // may not need this // Start driving forward, stopping if boundary sensors are triggered if(expl_substate == EXPL_START) { expl_substate = EXPL_DRIVE_FWD; expl_substate_count = 0; start_drive_guarded(HALF_SPEED, 100, 1500); // Long time out return; } if(expl_substate == EXPL_DRIVE_FWD) { // Wait until the drive forward guarded behavior stops, // then go to backup behavior for 3 seconds 1 s pause, 2 s backup. if(Motion_state == ST_STOP) { expl_collision_count++; expl_substate = EXPL_DRIVE_RVS; expl_substate_count = 0; start_drive_rvs(QUARTER_SPEED, 100, 200); } return; } if(expl_substate == EXPL_DRIVE_RVS) { // When backup is completed, find out what triggered the forward stop, // and initiate appropriate rotation if(Motion_state == ST_STOP) { expl_halt_reason = get_drive_halt_reason(); // Halt exploration activity after 4 collisions - just to get us out of explore during testing if(expl_collision_count > 4) { Activity_state = ST_REST; start_rest(); return; } if(expl_halt_reason == HALT_LEFT_WHISKER) { expl_substate = EXPL_ROTATE_CLOCK; expl_substate_count = 0; start_rotate_clock(QUARTER_SPEED, 100, 220); // About 90 degrees return; } if(expl_halt_reason == HALT_RIGHT_WHISKER) { expl_substate = EXPL_ROTATE_COUNTER; expl_substate_count = 0; start_rotate_counter(QUARTER_SPEED, 100, 220); return; } if(expl_halt_reason == HALT_DROPOFF) { expl_substate = EXPL_ROTATE_COUNTER; expl_substate_count = 0; start_rotate_counter(QUARTER_SPEED, 100, 220); // could do something else like turn around 180 return; } if(expl_halt_reason == HALT_TIMEOUT) { expl_substate = EXPL_ROTATE_COUNTER; expl_substate_count = 0; start_rotate_counter(QUARTER_SPEED, 100, 220); // or maybe we should go to stop return; } // Otherwise? shouldn't happen, but what if buggy? // Maybe backup again as error indicator? Or maybe terminate behavior //expl_substate = EXPL_DRIVE_RVS; //expl_substate_count = 0; //start_drive_rvs(QUARTER_SPEED, 100, 200); } return; } if(expl_substate == EXPL_ROTATE_CLOCK) { // Wait until the rotation behavior terminates, and go back to drive_guarded if(Motion_state == ST_STOP) { expl_substate = EXPL_DRIVE_FWD; expl_substate_count = 0; start_drive_guarded(HALF_SPEED, 100, 1500); } return; } if(expl_substate == EXPL_ROTATE_COUNTER) // currently mergeable with previous condition { // Wait until the rotation behavior terminates, and go back to drive_guarded if(Motion_state == ST_STOP) { expl_substate = EXPL_DRIVE_FWD; expl_substate_count = 0; start_drive_guarded(HALF_SPEED, 100, 1500); } return; } // And what if the system is in no recognizable explore state? // Should not happen, but maybe go back to rest after a long time? if(Activity_state_count > 10000) // 100 seconds { Activity_state = ST_REST; start_rest(); return; } } //---------------------------------------------------------------------------------------------