At each instersection I play different music for the compass direction the robot decided to go. Any ideas how to get the robot to speak the words "West", "East", "South", "North"?
if (current_direction == 'W')
OrangutanBuzzer::play("!T240 L8 a gafaeada");
if (current_direction == 'E')
OrangutanBuzzer::play("!T240 L8 a gfgfgfgf");
if (current_direction == 'S')
OrangutanBuzzer::play("!T240 L8 a efgefgef");
if (current_direction == 'N')
OrangutanBuzzer::play("!T240 L8 a abcabcab");
Full code:
// The following libraries will be needed by this demo
#include <Pololu3pi.h>
#include <PololuQTRSensors.h>
#include <OrangutanMotors.h>
#include <OrangutanAnalog.h>
#include <OrangutanLEDs.h>
#include <OrangutanLCD.h>
#include <OrangutanPushbuttons.h>
#include <OrangutanBuzzer.h>
Pololu3pi robot;
unsigned int sensors[5]; // an array to hold sensor values
int last1 = 0;
int last2 = 0;
int last3 = 0;
int stop_robot = 0;
int already_stopped = 0;
int use_pattern = 0;
char pattern[] = {'S', 'S', 'S', 'S', 'S', 'S', 'S', 'R', 'R', 'S', 'S', 'S', 'S', 'S', 'S', 'S', 'L', 'L', 'S', 'S', 'S', 'S', 'S', 'S', 'S', 'R', 'R', 'S', 'S', 'S', 'S', 'S', 'S', 'S', 'R', 'S', 'S', 'S', 'R'};
int pattern_length = 39;
int i = -1;
int pattern_loop = 1;
// This include file allows data to be stored in program space. The
// ATmega168 has 16k of program space compared to 1k of RAM, so large
// pieces of static data should be stored in program space.
#include <avr/pgmspace.h>
// Introductory messages. The "PROGMEM" identifier causes the data to
// go into program space.
const char welcome_line1[] PROGMEM = "Tim's";
const char welcome_line2[] PROGMEM = "Self";
const char demo_name_line1[] PROGMEM = "Driving";
const char demo_name_line2[] PROGMEM = "Robot";
// A couple of simple tunes, stored in program space.
const char welcome[] PROGMEM = ">g32>>c32";
const char go[] PROGMEM = "L16 cdegreg4";
const char melody[] PROGMEM = "!L16 V8 cdefgab>cbagfedc";
int play_intersection_music = 0;
int be_random = 1;
int speed = 1;
int stop_protection = 0;
int do_calibration = 0;
int variable_speed = 0;
int do_compass = 1;
char current_direction = 'W';
// Data for generating the characters used in load_custom_characters
// and display_readings. By reading levels[] starting at various
// offsets, we can generate all of the 7 extra characters needed for a
// bargraph. This is also stored in program space.
const char levels[] PROGMEM = {
0b00000,
0b00000,
0b00000,
0b00000,
0b00000,
0b00000,
0b00000,
0b11111,
0b11111,
0b11111,
0b11111,
0b11111,
0b11111,
0b11111
};
// This function loads custom characters into the LCD. Up to 8
// characters can be loaded; we use them for 7 levels of a bar graph.
void load_custom_characters()
{
OrangutanLCD::loadCustomCharacter(levels + 0, 0); // no offset, e.g. one bar
OrangutanLCD::loadCustomCharacter(levels + 1, 1); // two bars
OrangutanLCD::loadCustomCharacter(levels + 2, 2); // etc...
OrangutanLCD::loadCustomCharacter(levels + 3, 3);
OrangutanLCD::loadCustomCharacter(levels + 4, 4);
OrangutanLCD::loadCustomCharacter(levels + 5, 5);
OrangutanLCD::loadCustomCharacter(levels + 6, 6);
OrangutanLCD::clear(); // the LCD must be cleared for the characters to take effect
}
// This function displays the sensor readings using a bar graph.
void display_readings(const unsigned int *calibrated_values)
{
unsigned char i;
for (i=0;i<5;i++) {
// Initialize the array of characters that we will use for the
// graph. Using the space, an extra copy of the one-bar
// character, and character 255 (a full black box), we get 10
// characters in the array.
const char display_characters[10] = {
' ', 0, 0, 1, 2, 3, 4, 5, 6, 255 };
// The variable c will have values from 0 to 9, since
// calibrated values are in the range of 0 to 1000, and
// 1000/101 is 9 with integer math.
char c = display_characters[calibrated_values[i] / 101];
// Display the bar graph character.
OrangutanLCD::print(c);
}
}
void no_stopping() {
OrangutanLCD::clear();
OrangutanLCD::print("No\nStopping");
delay(1000);
}
// Initializes the 3pi, displays a welcome message, calibrates, and
// plays the initial music. This function is automatically called
// by the Arduino framework at the start of program execution.
void setup()
{
unsigned int counter; // used as a simple timer
// This must be called at the beginning of 3pi code, to set up the
// sensors. We use a value of 2000 for the timeout, which
// corresponds to 2000*0.4 us = 0.8 ms on our 20 MHz processor.
robot.init(2000);
load_custom_characters(); // load the custom characters
// Play welcome music and display a message
OrangutanLCD::printFromProgramSpace(welcome_line1);
OrangutanLCD::gotoXY(0, 1);
OrangutanLCD::printFromProgramSpace(welcome_line2);
OrangutanBuzzer::playFromProgramSpace(welcome);
delay(1000);
OrangutanLCD::clear();
OrangutanLCD::printFromProgramSpace(demo_name_line1);
OrangutanLCD::gotoXY(0, 1);
OrangutanLCD::printFromProgramSpace(demo_name_line2);
delay(1000);
OrangutanLCD::clear();
int bat = OrangutanAnalog::readBatteryMillivolts();
OrangutanLCD::print(bat);
OrangutanLCD::print("mV");
OrangutanLCD::gotoXY(0, 1);
delay(2000);
OrangutanLCD::clear();
if (!play_intersection_music) {
//OrangutanLCD::clear();
OrangutanLCD::print("No");
OrangutanLCD::gotoXY(0, 1);
OrangutanLCD::print("Stopping");
}
else {
//OrangutanLCD::clear();
OrangutanLCD::print("Do");
OrangutanLCD::gotoXY(0, 1);
OrangutanLCD::print("Stop");
}
// Display battery voltage and wait for button press
while (!OrangutanPushbuttons::isPressed(BUTTON_B))
{
//delay(100);
if (OrangutanPushbuttons::isPressed(BUTTON_A)) {
OrangutanPushbuttons::waitForRelease(BUTTON_A);
OrangutanLCD::clear();
if (play_intersection_music) {
play_intersection_music = 0;
//OrangutanLCD::clear();
OrangutanLCD::print("No");
OrangutanLCD::gotoXY(0, 1);
OrangutanLCD::print("Stopping");
}
else {
play_intersection_music = 1;
OrangutanLCD::print("Do");
OrangutanLCD::gotoXY(0, 1);
OrangutanLCD::print("Stop");
}
}
}
OrangutanPushbuttons::waitForRelease(BUTTON_B);
delay(1000);
// Always wait for the button to be released so that 3pi doesn't
// start moving until your hand is away from it.
// Auto-calibration: turn right and left while calibrating the
// sensors.
for (counter=0; counter<80; counter++)
{
if (counter < 20 || counter >= 60)
OrangutanMotors::setSpeeds(40, -40);
else
OrangutanMotors::setSpeeds(-40, 40);
// This function records a set of sensor readings and keeps
// track of the minimum and maximum values encountered. The
// IR_EMITTERS_ON argument means that the IR LEDs will be
// turned on during the reading, which is usually what you
// want.
robot.calibrateLineSensors(IR_EMITTERS_ON);
// Since our counter runs to 80, the total delay will be
// 80*20 = 1600 ms.
delay(20);
}
OrangutanMotors::setSpeeds(0, 0);
OrangutanLCD::clear();
OrangutanLCD::print("Go!");
// Play music and wait for it to finish before we start driving.
OrangutanBuzzer::playFromProgramSpace(go);
while(OrangutanBuzzer::isPlaying());
}
// This function, causes the 3pi to follow a segment of the maze until
// it detects an intersection, a dead end, or the finish.
void follow_segment()
{
int last_proportional = 0;
long integral=0;
while(1)
{
// Normally, we will be following a line. The code below is
// similar to the 3pi-linefollower-pid example, but the maximum
// speed is turned down to 60 for reliability.
// Get the position of the line.
unsigned int position = robot.readLine(sensors, IR_EMITTERS_ON);
if (!do_compass)
display_sensors();
// The "proportional" term should be 0 when we are on the line.
int proportional = ((int)position) - 2000;
// Compute the derivative (change) and integral (sum) of the
// position.
int derivative = proportional - last_proportional;
integral += proportional;
// Remember the last position.
last_proportional = proportional;
// Compute the difference between the two motor power settings,
// m1 - m2. If this is a positive number the robot will turn
// to the left. If it is a negative number, the robot will
// turn to the right, and the magnitude of the number determines
// the sharpness of the turn.
int power_difference = proportional/20 + integral/10000 + derivative*3/2;
// Compute the actual motor settings. We never set either motor
// to a negative value.
if (variable_speed)
speed = random(1, 7);
const int maximum = speed * 30; // the maximum speed
if (power_difference > maximum)
power_difference = maximum;
if (power_difference < -maximum)
power_difference = -maximum;
if (power_difference < 0)
OrangutanMotors::setSpeeds(maximum + power_difference, maximum);
else
OrangutanMotors::setSpeeds(maximum, maximum - power_difference);
// We use the inner three sensors (1, 2, and 3) for
// determining whether there is a line straight ahead, and the
// sensors 0 and 4 for detecting lines going to the left and
// right.
//if (sensors[0] > 200 && sensors[1] > 200 && sensors[2] > 200 && sensors[3] > 200 & sensors[4]) {
// OrangutanBuzzer::playFromProgramSpace(go);
// while(OrangutanBuzzer::isPlaying());
//}
if (sensors[1] < 100 && sensors[2] < 100 && sensors[3] < 100)
{
// There is no line visible ahead, and we didn't see any
// intersection. Must be a dead end.
return;
}
else if (sensors[0] > 200 || sensors[4] > 200)
{
// Found an intersection.
return;
}
}
}
// Code to perform various types of turns according to the parameter dir,
// which should be 'L' (left), 'R' (right), 'S' (straight), or 'B' (back).
// The delays here had to be calibrated for the 3pi's motors.
void turn(unsigned char dir)
{
if (play_intersection_music == 1) {
OrangutanMotors::setSpeeds(0, 0);
OrangutanBuzzer::playFromProgramSpace(melody);
while(OrangutanBuzzer::isPlaying());
}
switch(dir)
{
case 'L':
// Turn left.
OrangutanMotors::setSpeeds(-80, 80);
delay(200);
if (current_direction == 'W')
current_direction = 'S';
else if (current_direction == 'E')
current_direction = 'N';
else if (current_direction == 'S')
current_direction = 'E';
else if (current_direction == 'N')
current_direction = 'W';
break;
case 'R':
// Turn right.
if (current_direction == 'W')
current_direction = 'N';
else if (current_direction == 'E')
current_direction = 'S';
else if (current_direction == 'S')
current_direction = 'W';
else if (current_direction == 'N')
current_direction = 'E';
OrangutanMotors::setSpeeds(80, -80);
delay(200);
if (do_calibration) {
int counter;
for (counter=0; counter<80; counter++)
{
if (counter < 20 || counter >= 60)
OrangutanMotors::setSpeeds(40, -40);
else
OrangutanMotors::setSpeeds(-40, 40);
// This function records a set of sensor readings and keeps
// track of the minimum and maximum values encountered. The
// IR_EMITTERS_ON argument means that the IR LEDs will be
// turned on during the reading, which is usually what you
// want.
robot.calibrateLineSensors(IR_EMITTERS_ON);
// Since our counter runs to 80, the total delay will be
// 80*20 = 1600 ms.
delay(20);
}
}
OrangutanMotors::setSpeeds(0, 0);
break;
case 'B':
// Turn around.
if (current_direction == 'W')
current_direction = 'E';
else if (current_direction == 'E')
current_direction = 'W';
else if (current_direction == 'S')
current_direction = 'N';
else if (current_direction == 'N')
current_direction = 'S';
OrangutanMotors::setSpeeds(80, -80);
delay(400);
break;
case 'S':
// Don't do anything!
break;
}
}
// The path variable will store the path that the robot has taken. It
// is stored as an array of characters, each of which represents the
// turn that should be made at one intersection in the sequence:
// 'L' for left
// 'R' for right
// 'S' for straight (going straight through an intersection)
// 'B' for back (U-turn)
//
// Whenever the robot makes a U-turn, the path can be simplified by
// removing the dead end. The follow_next_turn() function checks for
// this case every time it makes a turn, and it simplifies the path
// appropriately.
char path[100] = "";
unsigned char path_length = 0; // the length of the path
void display_sensors()
{
unsigned int position = robot.readLine(sensors, IR_EMITTERS_ON);
// Display the position measurement, which will go from 0
// (when the leftmost sensor is over the line) to 4000 (when
// the rightmost sensor is over the line) on the 3pi, along
// with a bar graph of the sensor readings. This allows you
// to make sure the robot is ready to go.
OrangutanLCD::clear();
OrangutanLCD::print(sensors[0]/10);
//OrangutanLCD::print(sensors[1]/10);
//OrangutanLCD::print(sensors[2]/10);
//OrangutanLCD::print(sensors[3]/10);
//OrangutanLCD::print(sensors[4]/10);
OrangutanLCD::gotoXY(0, 1);
display_readings(sensors);
}
// Displays the current path on the LCD, using two rows if necessary.
void display_path()
{
// Set the last character of the path to a 0 so that the print()
// function can find the end of the string. This is how strings
// are normally terminated in C.
path[path_length] = 0;
OrangutanLCD::clear();
OrangutanLCD::print(path);
if (path_length > 8)
{
OrangutanLCD::gotoXY(0, 1);
OrangutanLCD::print(path + 8);
}
}
// This function decides which way to turn during the learning phase of
// maze solving. It uses the variables found_left, found_straight, and
// found_right, which indicate whether there is an exit in each of the
// three directions, applying the "left hand on the wall" strategy.
unsigned char select_turn(unsigned char found_left, unsigned char found_straight, unsigned char found_right)
{
i = i + 1;
if (use_pattern) {
if (pattern_loop) {
return pattern[i % pattern_length];
}
else if (i >= pattern_length) {
stop_robot = 1;
return 'B';
} else {
return pattern[i];
}
}
// Make a decision about how to turn. The following code
// implements a left-hand-on-the-wall strategy, where we always
// turn as far to the left as possible.
if (be_random == 1) {
if (found_left && found_straight && found_right) {
int picked_choice = random(0, 4);
if (picked_choice == 0)
return 'B';
if (picked_choice == 1)
return 'L';
if (picked_choice == 2)
return 'S';
if (picked_choice == 3)
return 'R';
}
if (found_left && found_straight && !found_right) {
int picked_choice = random(0, 3);
if (picked_choice == 0)
return 'B';
if (picked_choice == 1)
return 'L';
if (picked_choice == 2)
return 'S';
}
if (found_left && !found_straight && found_right) {
int picked_choice = random(0, 3);
if (picked_choice == 0)
return 'B';
if (picked_choice == 1)
return 'L';
if (picked_choice == 2)
return 'R';
}
if (found_left && !found_straight && !found_right) {
int picked_choice = random(0, 2);
if (picked_choice == 0)
return 'B';
if (picked_choice == 1)
return 'L';
}
if (!found_left && found_straight && found_right) {
int picked_choice = random(0, 3);
if (picked_choice == 0)
return 'B';
if (picked_choice == 1)
return 'S';
if (picked_choice == 2)
return 'R';
}
if (!found_left && found_straight && !found_right) {
int picked_choice = random(0, 2);
if (picked_choice == 0)
return 'B';
if (picked_choice == 1)
return 'S';
}
if (!found_left && !found_straight && found_right) {
int picked_choice = random(0, 2);
if (picked_choice == 0)
return 'B';
if (picked_choice == 1)
return 'R';
}
if (last1 == 0)
last1 = 1;
else if (last2 == 0)
last2 = 1;
else if (last3 == 0)
last3 = 1;
else
stop_robot = 1;
return 'B';
}
else {
if (found_left)
return 'L';
else if (found_straight)
return 'S';
else if (found_right)
return 'R';
else
return 'B';
}
}
// Path simplification. The strategy is that whenever we encounter a
// sequence xBx, we can simplify it by cutting out the dead end. For
// example, LBL -> S, because a single S bypasses the dead end
// represented by LBL.
void simplify_path()
{
// only simplify the path if the second-to-last turn was a 'B'
if (path_length < 3 || path[path_length-2] != 'B')
return;
int total_angle = 0;
int i;
for (i = 1; i <= 3; i++)
{
switch (path[path_length - i])
{
case 'R':
total_angle += 90;
break;
case 'L':
total_angle += 270;
break;
case 'B':
total_angle += 180;
break;
}
}
// Get the angle as a number between 0 and 360 degrees.
total_angle = total_angle % 360;
// Replace all of those turns with a single one.
switch (total_angle)
{
case 0:
path[path_length - 3] = 'S';
break;
case 90:
path[path_length - 3] = 'R';
break;
case 180:
path[path_length - 3] = 'B';
break;
case 270:
path[path_length - 3] = 'L';
break;
}
// The path is now two steps shorter.
path_length -= 2;
}
// This function comprises the body of the maze-solving program. It is called
// repeatedly by the Arduino framework.
void loop()
{
if (!stop_robot || !stop_protection) {
follow_segment();
// Drive straight a bit. This helps us in case we entered the
// intersection at an angle.
// Note that we are slowing down - this prevents the robot
// from tipping forward too much.
OrangutanMotors::setSpeeds(50, 50);
delay(50);
if (!do_compass)
display_sensors();
// These variables record whether the robot has seen a line to the
// left, straight ahead, and right, whil examining the current
// intersection.
unsigned char found_left = 0;
unsigned char found_straight = 0;
unsigned char found_right = 0;
// Now read the sensors and check the intersection type.
unsigned int sensors[5];
robot.readLine(sensors, IR_EMITTERS_ON);
// Check for left and right exits.
if (sensors[0] > 100)
found_left = 1;
if (sensors[4] > 100)
found_right = 1;
// Drive straight a bit more - this is enough to line up our
// wheels with the intersection.
OrangutanMotors::setSpeeds(40, 40);
delay(200);
// Check for a straight exit.
robot.readLine(sensors, IR_EMITTERS_ON);
if (sensors[1] > 200 || sensors[2] > 200 || sensors[3] > 200)
found_straight = 1;
// Intersection identification is complete.
// If the maze has been solved, we can follow the existing
// path. Otherwise, we need to learn the solution.
unsigned char dir = select_turn(found_left, found_straight, found_right);
// Make the turn indicated by the path.
turn(dir);
if (do_compass) {
OrangutanMotors::setSpeeds(0, 0);
OrangutanLCD::clear();
OrangutanLCD::print(dir);
OrangutanLCD::gotoXY(0, 1);
OrangutanLCD::print(current_direction);
if (current_direction == 'W')
OrangutanBuzzer::play("!T240 L8 a gafaeada");
if (current_direction == 'E')
OrangutanBuzzer::play("!T240 L8 a gfgfgfgf");
if (current_direction == 'S')
OrangutanBuzzer::play("!T240 L8 a efgefgef");
if (current_direction == 'N')
OrangutanBuzzer::play("!T240 L8 a abcabcab");
while(OrangutanBuzzer::isPlaying());
}
// Store the intersection in the path variable.
path[path_length] = dir;
path_length++;
// You should check to make sure that the path_length does not
// exceed the bounds of the array. We'll ignore that in this
// example.
// Simplify the learned path.
simplify_path();
// Display the path on the LCD.
//display_path();
}
else {
OrangutanMotors::setSpeeds(0, 0);
if (!already_stopped) {
OrangutanLCD::clear();
OrangutanLCD::print("Stopped");
already_stopped = 1;
}
}
}
