// Heartbeat and state machine boilerplate 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 form do_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 micro-controller heartbeat, here running at 100 Hz. // Of course nothing is really going on simultaneously, it's effectively fine-grained time-sharing. // Take some time to understand how this basic program architecture works. Unless you have taken // an OS implementation course, it may not resemble anything you have ever seen, since the primary concern // is not getting a computational result, but interlacing precisely in time a bunch of operations that // are all conceptually happening concurrently, but that all have specific time constraints and // temporal dependencies on each other. // I would encourage you to extend the basic architecture when you start implementing your own behaviors. // It may seem confusing initially, and it is not as elegantly implemented as a true // real-time operating system, but it has been refined over 5 generations of this course, and has run, // in various forms, all the robots we have built. It is quite flexible, adapts naturally to // bots with hierarchical multi-processor architectures, and can be generalized to a hierarchical // state machine for implementing more sophisticated behaviors (I'll provide an example of this later) // Pin definitions for Arduino Uno // Using these pins might be a good idea. I selected them so that connections between the // Arduino and the breadboard could be made using ribbon cables rather than individual wires, which // are more susceptible to being dislodged, and are more difficult to keep track of. //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 // 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 with 6 Volts #define MIN_SET 900 // about 60 rpm counterclockwise #define MAX_SPEED 600 // Values are variation about center set point (1500) #define HALF_SPEED 300 #define QUARTER_SPEED 150 int bot_speed = 0; // Allows bot speed to be set globally // 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; // States. What is the bot is doing. // Only five for now. #define ST_STOP 10 #define ST_DRIVE_FWD 20 #define ST_DRIVE_RVS 21 #define ST_ROTATE_CLOCK 30 #define ST_ROTATE_COUNTER 31 int current_state = ST_STOP; int current_state_count = 0; // Cycles system has been in current state // Heartbeat timing #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 count100 = 0; // One second at 100 Hz heartbeat unsigned int count2 = 0; unsigned int count5 = 0; unsigned int count10 = 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); } //*************************************************************************************** // 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; count100 = 0; count2 = 0; count5 = 0; count10 = 0; count1600 = 0; current_state = ST_STOP; current_state_count = 0; 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 // In general this would be a read-sensors routine read_whiskers(); // Branch according to current state // This is how state behavior is implemented if(current_state == ST_STOP) do_stop(); else if(current_state == ST_DRIVE_FWD) do_drive_fwd(); else if(current_state == ST_DRIVE_RVS) do_drive_rvs(); else if(current_state == ST_ROTATE_CLOCK) do_rotate_clock(); else if(current_state == ST_ROTATE_COUNTER) do_rotate_counter(); // Recovery if system somehow got into an unknown state... else current_state = ST_STOP; // Use LEDs to display information about what is going on led_display(); // 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++; count100++; if(count100 >= 100) count100 = 0; count2++; if(count2 >= 2) count2 = 0; count5++; if(count5 >= 5) count5 = 0; count10++; if(count10 >= 10) count10 = 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 led_display() // Flash LEDs to indicate various state information // Called every heartbeat cycle // Freely modifiable { // Start by turning everything off. // Some will be turned back on almost immediately with imperceptible flicker. digitalWrite(RED_LED, LOW); digitalWrite(YELLOW_LED, LOW); digitalWrite(GREEN_LED, LOW); digitalWrite(BLUE_LED, LOW); // The following aspects of the routine can be freely modified to display // whatever is needed for current development, or cool or interesting // Flash brief yellow to indicate stop mode if(current_state == ST_STOP) { if(count100 < 10) digitalWrite(YELLOW_LED, HIGH); } // Blink green to indicate drive_fwd mode if(current_state == ST_DRIVE_FWD) { if(count100 < 50) digitalWrite(GREEN_LED, HIGH); } // Blink green twice fast to indicate drive_rvs mode if(current_state == ST_DRIVE_RVS) { if(count100 < 10) digitalWrite(GREEN_LED, HIGH); if(count100 >= 25 && count100 < 35) digitalWrite(GREEN_LED, HIGH); } // Blink blue to indicate rotate clockwise mode if(current_state == ST_ROTATE_CLOCK) { if(count100 < 50) digitalWrite(BLUE_LED, HIGH); } // Blink blue twice fast to indicate rotate counterclockwise mode if(current_state == ST_ROTATE_COUNTER) { if(count100 < 10) digitalWrite(BLUE_LED, HIGH); if(count100 >= 25 && count100 < 35) 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); } //--------------------------------------------------------------------------------------------- //--------------------------------------------------------------------------------------------- // Routines implementing behavioral details of various states void do_stop() // In the stop state, both wheel speeds are set to 0 // State can transition to drive_fwd, drive_rvs, rotate_clock, and rotate_counter // Accesses global variables current_state, and current_state_count { if(current_state != ST_STOP) return; // If somehow we got here by mistake... current_state_count++; // increment count of heartbeats we have been in the current state // 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(current_state_count < 100) return; // As test, leave stop if whiskers are pressed // In this initial demo, bot cycles through drive forward, reverse, rotate clockwise, counterclockwise // Right whisker jumps into sequence at drive-reverse, illustrating flexibility if(cur_left_whisker == HIGH) { current_state = ST_DRIVE_FWD; current_state_count = 0; return; } if(cur_right_whisker == HIGH) { current_state = ST_DRIVE_RVS; current_state_count = 0; return; } // Otherwise, we just stay in stop return; } // For initial demo, the bot cycles through forward, reverse, clockwise, and counterclockwise // movement states, and then returns to the stop state void do_drive_fwd() // In the stop state, both wheel speeds are set to bot_speed // Accesses global variables current_state, and current_state_count { if(current_state != ST_DRIVE_FWD) return; // If somehow we got here by mistake... current_state_count++; // increment count of heartbeats we have been in the current state // Don't move for the first second if(current_state_count < 100) { 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 + bot_speed, CENTER_SET - bot_speed); } // For initial tdemo, after 3 seconds (2 seconds of driving) go to the reverse state. if(current_state_count >= 300) { // might want to stop motors for safety, even though the next state does it in demo mode current_state = ST_DRIVE_RVS; // demo cycle through simple behaviors current_state_count = 0; return; } } void do_drive_rvs() // In the revers state, both wheel speeds are set to - bot_speed // Accesses global variables current_state, and current_state_count { if(current_state != ST_DRIVE_RVS) return; // If somehow we got here by mistake... current_state_count++; // increment count of heartbeats we have been in the current state // Don't move for the first second = 100 heartbeats if(current_state_count < 100) { 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 - bot_speed, CENTER_SET + bot_speed); } // For initial demo, after 3 seconds (2 seconds of driving) go to the rotate clockwise state. if(current_state_count >= 300) { current_state = ST_ROTATE_CLOCK; current_state_count = 0; return; } } void do_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 current_state, and current_state_count { if(current_state != ST_ROTATE_CLOCK) return; // If somehow we got here by mistake... current_state_count++; // increment count of heartbeats we have been in the current state // Don't move for the first second if(current_state_count < 100) { 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 - bot_speed, CENTER_SET - bot_speed); } // For initial demo, after 3 seconds (2 seconds of motion) we go to the rotate counter state. if(current_state_count >= 300) { current_state = ST_ROTATE_COUNTER; current_state_count = 0; return; } } void do_rotate_counter() // In the rotate counter(clockwise) state, both wheel speeds are set to rotate clockwise // at bot_speed // Accesses global variables current_state, and current_state_count { if(current_state != ST_ROTATE_COUNTER) return; // If somehow we got here by mistake... current_state_count++; // increment count of heartbeats we have been in the current state // Don't move for the first second if(current_state_count < 100) { 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 + bot_speed, CENTER_SET + bot_speed); } // For initial testing, after 3 seconds (2 seconds of motion) we enter go back to the stop state. if(current_state_count >= 300) { current_state = ST_STOP; current_state_count = 0; return; } } //---------------------------------------------------------------------------------------------