Hello! I just got my 3pi about a week ago and I love it. It’s far better than most robots I have build over the last 3 years. 
I most use Arduino boards for my robots and program with the Arduino software. But instead of using that to program the 3pi I decided to use AVR studios as the programer. It’s harder to then Arduino but I have more programing options with AVR studios . I wanted to add more options to the original demo program Pololu provided. Such as line following, smart move so it does not run into objects, maze solving, spinning with LED’s turned on for a cool show at night, move, spinning line following, and hopefully more soon. What most of these thing do are in the code.
I need help writing the code, what I have done so far is at the bottom this is my first time writing a code on AVR studios. Please help!
/*
3pi-main-program - code for the Pololu 3pi Robot
Xamran Shamsi
xamran.shamsi@gmail.com
*/
// The 3pi include file must be at the beginning of any program that
// uses the Pololu AVR library and 3pi.
#include <pololu/3pi.h>
// 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 = "Xamran's";
const char welcome_line2[] PROGMEM = "3\xf7 Robot"; // \xf7 is a greek
// pi character
const char main_program_name_line1[] PROGMEM = "Main";
const char main_program_line2[] PROGMEM = "Program";
const char instructions_line1[] PROGMEM = "Press B";
const char instructions_line2[] PROGMEM = "to start";
const char pi_on_line1[] PROGMEM = "3\xf7";
const char pi_on_line2[] PROGMEM = "On";
const char main_menu_intro_line1[] PROGMEM = "Main";
const char main_menu_intro_line2[] PROGMEM = "Menu";
const char menu_smart_move_line1[] PROGMEM = Smart
const char menu_smart_move_line2[] PROGMEM = Move
const char menu_move[] PROGMEM = "Move";
const char menu_spinning_line_line1[] PROGMEM = "Spinning";
const char menu_spinning_line_line2[] PROGMEM = "Line";
const char menu_spinning_line_line3[] PROGMEM = "Follower"
const char menu_spin[] PROGMEM = "Spin"
const char menu_maze_solving_line1[] PROGMEM = "Maze";
const char menu_maze_solving_line2[] PROGMEM = "Solving";
const char menu_line_following_line1[] PROGMEM = "Line";
const char menu_line_following_line2[] PROGMEM = "Following";
const char menu_bat_test[] PROGMEM = "Battery";
const char menu_led_test[] PROGMEM = "LEDs";
const char menu_lcd_test[] PROGMEM = "LCD";
const char menu_ir_test[] PROGMEM = "Sensors";
const char menu_motor_test[] PROGMEM = "Motors";
const char menu_music_test[] PROGMEM = "Music";
const char menu_pot_test[] PROGMEM = "Trimpot";
const char menu_time_test[] PROGMEM = "Timer";
const char menu_line2[] PROGMEM = "\x7f" "A \xa5" "B C\x7e";
const char back_line2[] PROGMEM = "\6B";
void bat_test();
void led_test();
void lcd_test();
void ir_test();
void motor_test();
void music_test();
void time_test();
void pot_test();
typedef void (*function)();
const function main_menu_functions[] = { smart_move, spin, move, spin_line, line_following, maze_solving, bat_test, led_test, pot_test, ir_test, motor_test, music_test, time_test };
const char *main_menu_options[] = { menu_smart_move, menu_spin, menu_move, menu_spin_line, menu_line_following, menu_maze_solving, menu_bat_test, menu_led_test, menu_pot_test, menu_ir_test, menu_motor_test, menu_music_test, menu_time_test };
const char main_menu_length = sizeof(main_menu_options)/sizeof(main_menu_options[0]);
// A couple of simple tunes, stored in program space.
const char welcome[] PROGMEM = ">g32>>c32";
const char pi_on_music[] PROGMEM = ">>c32>g32";
const char beep_button_a[] PROGMEM = "!c32";
const char beep_button_b[] PROGMEM = "!e32";
const char beep_button_c[] PROGMEM = "!g32";
const char timer_tick[] PROGMEM = "!v8>>c32";
// 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 character is a musical note.
const prog_char note[] PROGMEM = {
0b00100,
0b00110,
0b00101,
0b00101,
0b00100,
0b11100,
0b11100,
0b00000,
};
// This character is a back arrow.
const prog_char back_arrow[] PROGMEM = {
0b00000,
0b00010,
0b00001,
0b00101,
0b01001,
0b11110,
0b01000,
0b00100,
};
// This function loads custom characters into the LCD. Up to 8
// characters can be loaded; we use them for 6 levels of a bar graph
// plus a back arrow and a musical note character.
void load_custom_characters()
{
lcd_load_custom_character(levels+0,0); // no offset, e.g. one bar
lcd_load_custom_character(levels+1,1); // two bars
lcd_load_custom_character(levels+2,2); // etc...
lcd_load_custom_character(levels+4,3); // skip level 3
lcd_load_custom_character(levels+5,4);
lcd_load_custom_character(levels+6,5);
lcd_load_custom_character(back_arrow,6);
lcd_load_custom_character(note,7);
clear(); // the LCD must be cleared for the characters to take effect
}
// 10 levels of bar graph characters
const char bar_graph_characters[10] = {' ',0,0,1,2,3,3,4,5,255};
void smart_move
//I had in mind of using a PING))) Ultrasonic Range Finder so it would move around
//with out running into thing.
#ifndef OrangutanMotors_h
#define OrangutanMotors_h
{
if(button_is_pressed(BUTTON_B))
{
do
set_m1_speed(255);
set_m2_speed(255);
set_motors(255,255);
{
else
set_m1_speed(0);
set_m2_speed(0);
set_motors(0,0);
}
if //ping sees somthing move out of the way
{
if(button_is_pressed(BUTTON_B))
return 1;
}
return 0;
}
void move()
{
if (button_is_pressed(BUTTON_(A))//go forward
do
set_m1_speed(255);
set_m2_speed(255);
set_motors(255,255);
{
else
set_m1_speed(0);
set_m2_speed(0);
set_motors(0,0);
}
}
{
if (button_is_pressed(BUTTON_(C))//go backwards
do
set_m1_speed();
set_m2_speed(-255);
set_motors(255,255);
{
else
set_m1_speed(0);
set_m2_speed(0);
set_motors(0,0);
}
}
{
if(button_is_pressed(BUTTON_B))
return 1;
}
return 0;
}
void spin()
if (button_is_pressed(BUTTON_(A))
do
set_m1_speed(255);
set_m2_speed(-255);
set_motors(255,-255);
{
else
set_m1_speed(0);
set_m2_speed(0);
set_motors(0,0);
}
}
if (button_is_pressed(BUTTON_(C))
do
set_m1_speed(-255);
set_m2_speed(255);
set_motors(-255,255);
{
else
set_m1_speed(0);
set_m2_speed(0);
set_motors(0,0);
}
}
{
if(button_is_pressed(BUTTON_B))
return 1;
}
return 0;
}
void spinning_line
#define FORWARD_OFFSET 0xA0 // Offset (0..255) of forward from the the front line
#define MAX_SPEED 255 // Maximum speed the wheels will go
#define MIN_SPEED 200 // Minimum speed the wheels will go
void Spinning_Line_Follow( void )
{
unsigned short phase_start = get_ms(); // Start time of this rotation
unsigned short last_phase_len = 100; // Duration of the last rotation
char last_line_side = 0; // which side was the line on?
char line_count = 0; // Is this the front or back line?
char led_duration = 0; // How much longer should the LED be on
while ( 1 ) {
unsigned short cur_time = get_ms(); // Grab the current time in ms
unsigned int sensors[5]; // Is the line left or right?
char line_side = (read_line(sensors,IR_EMITTERS_ON) < 2000);
left_led( 0 ); // Turn off the "FRONT" LED
if (line_side & !last_line_side) { // If it just changed,
if ( ++line_count & 1 ) { // and if this is the front line
left_led( 1 ); // Turn on "FRONT" LED
last_phase_len = cur_time - phase_start;// save the last rotation duration
phase_start = cur_time; // and start counting this rotation
}
}
last_line_side = line_side; // Remember where the line was
unsigned short cur_phase = cur_time - phase_start; // How far are we into the curent rotation?
cur_phase <<= 8; // Multipy by 256
cur_phase /= last_phase_len; // based on the last rotation duration
cur_phase += FORWARD_OFFSET; // offset by which direction is "FORWARD"
short left = cur_phase & 0xFF; // Wrap back to 0 .. 255
if ( left >= 128 ) { // Convert to 0 .. 127 .. 0
left = 256 - left;
}
left = (((left * (MAX_SPEED - MIN_SPEED))>>7) + MIN_SPEED); // Scale the wheel speed to be MIN at 0, MAX at 127
short right = MAX_SPEED + MIN_SPEED - left; // the right is 180 degress out of phase from the left
set_motors(left, -right); // and the right goes backwards
}
}
void maze_solving
// I have no clue on how to do this.
void line_following
void initialize()
{
unsigned int counter; // used as a simple timer
unsigned int sensors[5]; // an array to hold sensor values
// 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.
pololu_3pi_init(2000);
load_custom_characters(); // load the custom characters
// Play welcome music and display a message
print_from_program_space(welcome_line1);
lcd_goto_xy(0,1);
print_from_program_space(welcome_line2);
play_from_program_space(welcome);
delay_ms(1000);
clear();
print_from_program_space(demo_name_line1);
lcd_goto_xy(0,1);
print_from_program_space(demo_name_line2);
delay_ms(1000);
// Display battery voltage and wait for button press
while(!button_is_pressed(BUTTON_B))
{
int bat = read_battery_millivolts();
clear();
print_long(bat);
print("mV");
lcd_goto_xy(0,1);
print("Press B");
delay_ms(100);
}
// Always wait for the button to be released so that 3pi doesn't
// start moving until your hand is away from it.
wait_for_button_release(BUTTON_B);
delay_ms(1000);
// Auto-calibration: turn right and left while calibrating the
// sensors.
for(counter=0;counter<80;counter++)
{
if(counter < 20 || counter >= 60)
set_motors(40,-40);
else
set_motors(-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.
calibrate_line_sensors(IR_EMITTERS_ON);
// Since our counter runs to 80, the total delay will be
// 80*20 = 1600 ms.
delay_ms(20);
}
set_motors(0,0);
// Display calibrated values as a bar graph.
while(!button_is_pressed(BUTTON_B))
{
// Read the sensor values and get the position measurement.
unsigned int position = read_line(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.
clear();
print_long(position);
lcd_goto_xy(0,1);
display_readings(sensors);
delay_ms(100);
}
wait_for_button_release(BUTTON_B);
clear();
print("Go!");
// Play music and wait for it to finish before we start driving.
play_from_program_space(go);
while(is_playing());
}
// This is the main function, where the code starts. All C programs
// must have a main() function defined somewhere.
int main()
{
unsigned int sensors[5]; // an array to hold sensor values
unsigned int last_proportional=0;
long integral=0;
// set up the 3pi
initialize();
// This is the "main loop" - it will run forever.
while(1)
{
// Get the position of the line. Note that we *must* provide
// the "sensors" argument to read_line() here, even though we
// are not interested in the individual sensor readings.
unsigned int position = read_line(sensors,IR_EMITTERS_ON);
// 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 right. If it is a negative number, the robot will
// turn to the left, 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.
const int max = 60;
if(power_difference > max)
power_difference = max;
if(power_difference < -max)
power_difference = -max;
if(power_difference < 0)
set_motors(max+power_difference, max);
else
set_motors(max, max-power_difference);
}
// This part of the code is never reached. A robot should
// never reach the end of its program, or unpredictable behavior
// will result as random code starts getting executed. If you
// really want to stop all actions at some point, set your motors
// to 0,0 and run the following command to loop forever:
//
// while(1);
}
// Displays the battery voltage.
void bat_test()
{
int bat = read_battery_millivolts();
print_long(bat);
print("mV");
delay_ms(100);
}
char wait_for_250_ms_or_button_b()
{
int i;
for(i=0;i<25;i++)
{
delay_ms(10);
if(button_is_pressed(BUTTON_B))
return 1;
}
return 0;
}
// Blinks the LEDs
void led_test()
{
play("c32");
print("Red, Red");
red_led(1);
if(wait_for_250_ms_or_button_b())
return;
red_led(0);
if(wait_for_250_ms_or_button_b())
return;
play(">c32");
lcd_goto_xy(0,0);
print("Green, Red");
green_led(1);
if(wait_for_250_ms_or_button_b())
return;
green_led(0);
if(wait_for_250_ms_or_button_b())
return;
}
void ir_test()
{
unsigned int sensors[5]; // an array to hold sensor values
if(button_is_pressed(BUTTON_C))
read_line_sensors(sensors, IR_EMITTERS_OFF);
else
read_line_sensors(sensors,IR_EMITTERS_ON);
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.
// The variable c will have values from 0 to 9, since
// values are in the range of 0 to 2000, and 2000/201 is 9
// with integer math.
char c = bar_graph_characters[sensors[i]/201];
// Display the bar graph characters.
print_character(c);
}
// Display an indicator of whether IR is on or off
if(button_is_pressed(BUTTON_C))
print("IR-");
else
print(" C");
delay_ms(100);
}
int m1_speed = 0;
int m2_speed = 0;
void motor_test()
{
static char m1_back = 0, m2_back = 0;
char m1_char, m2_char;
if(button_is_pressed(BUTTON_A))
{
if(m1_speed == 0)
{
delay_ms(200);
// If the button is pressed quickly when the motor is off,
// reverse direction.
if(!button_is_pressed(BUTTON_A))
m1_back = !m1_back;
}
m1_speed += 10;
}
else
m1_speed -= 20;
if(button_is_pressed(BUTTON_C))
{
if(m2_speed == 0)
{
delay_ms(200);
// If the button is pressed quickly when the motor is off,
// reverse direction.
if(!button_is_pressed(BUTTON_C))
m2_back = !m2_back;
}
m2_speed += 10;
}
else
m2_speed -= 20;
if(m1_speed < 0)
m1_speed = 0;
if(m1_speed > 255)
m1_speed = 255;
if(m2_speed < 0)
m2_speed = 0;
if(m2_speed > 255)
m2_speed = 255;
// 255/26 = 9, so this gives values in the range of 0 to 9
m1_char = bar_graph_characters[m1_speed / 26];
m2_char = bar_graph_characters[m2_speed / 26];
print_character(m1_char);
print_character(m1_back ? 'a' : 'A');
print_character(m1_char);
lcd_goto_xy(5,0);
print_character(m2_char);
print_character(m2_back ? 'c' : 'C');
print_character(m2_char);
set_motors(m1_speed * (m1_back ? -1 : 1), m2_speed * (m2_back ? -1 : 1));
delay_ms(50);
}
const char fugue[] PROGMEM =
"! T120O5L16agafaea dac+adaea fa<aa<bac#a dac#adaea f"
"O6dcd<b-d<ad<g d<f+d<gd<ad<b- d<dd<ed<f+d<g d<f+d<gd<ad"
"L8MS<b-d<b-d MLe-<ge-<g MSc<ac<a MLd<fd<f O5MSb-gb-g"
"ML>c#e>c#e MS afaf ML gc#gc# MS fdfd ML e<b-e<b-"
"O6L16ragafaea dac#adaea fa<aa<bac#a dac#adaea faeadaca"
"<b-acadg<b-g egdgcg<b-g <ag<b-gcf<af dfcf<b-f<af"
"<gf<af<b-e<ge c#e<b-e<ae<ge <fe<ge<ad<fd"
"O5e>ee>ef>df>d b->c#b->c#a>df>d e>ee>ef>df>d"
"e>d>c#>db>d>c#b >c#agaegfe fO6dc#dfdc#<b c#4";
const char fugue_title[] PROGMEM = " \7 Fugue in D Minor - by J.S. Bach \7 ";
void music_test()
{
static char fugue_title_pos = 0;
static long last_shift = 0;
char c,i;
if(get_ms() - last_shift > 250)
{
for(i=0;i<8;i++)
{
c = pgm_read_byte(fugue_title + fugue_title_pos + i);
print_character(c);
}
last_shift = get_ms();
fugue_title_pos ++;
if(fugue_title_pos + 8 >= sizeof(fugue_title))
fugue_title_pos = 0;
}
if(!is_playing())
{
play_from_program_space(fugue);
}
delay_ms(100);
}
void pot_test()
{
long start = get_ms();
char elapsed_ms;
int value;
set_analog_mode(MODE_10_BIT);
print_long(read_trimpot());
print(" "); // to clear the display
while((elapsed_ms = get_ms() - start) < 100)
{
value = read_trimpot();
play_frequency(value, 200, 15);
if(value < elapsed_ms*10)
{
red_led(0);
green_led(1);
}
else
{
red_led(1);
green_led(0);
}
}
}
void move()
if (button
void time_test()
{
static long elapsed_time = 0;
static long last_read = 0;
static long is_ticking = 0;
static char a_is_pressed = 0;
static char c_is_pressed = 0;
static char last_seconds = 0;
long current_time = get_ms();
if(is_ticking)
elapsed_time += current_time - last_read;
last_read = current_time;
if(button_is_pressed(BUTTON_A) && !a_is_pressed)
{
// reset
a_is_pressed = 1;
is_ticking = 0;
elapsed_time = 0;
if(!is_playing()) // only play once
play_from_program_space(beep_button_a);
}
// find the end of the button press without stopping
if(!button_is_pressed(BUTTON_A))
a_is_pressed = 0;
if(button_is_pressed(BUTTON_C) && !c_is_pressed)
{
// start/stop
c_is_pressed = 1;
is_ticking = !is_ticking;
play_from_program_space(beep_button_c);
}
// find the end of the button press without stopping
if(!button_is_pressed(BUTTON_C))
c_is_pressed = 0;
print_long((elapsed_time/1000/60/10)%10); // tens of minutes
print_long((elapsed_time/1000/60)%10); // minutes
print_character(':');
print_long((elapsed_time/1000)%60/10); // tens of seconds
char seconds = ((elapsed_time/1000)%60)%10;
print_long(seconds); // seconds
print_character('.');
print_long((elapsed_time/100)%10); // tenths of seconds
print_long((elapsed_time/10)%10); // hundredths of seconds
// beep every second
if(seconds != last_seconds && elapsed_time != 0 && !is_playing())
play_from_program_space(timer_tick);
last_seconds = seconds;
}
void print_two_lines_delay_1s(const char *line1, const char *line2)
{
// Play welcome music and display a message
clear();
print_from_program_space(line1);
lcd_goto_xy(0,1);
print_from_program_space(line2);
delay_ms(1000);
}
// waits for a button, plays the appropriate beep, and returns the
// button or buttons that were pressed
char wait_for_button_and_beep()
{
char button = wait_for_button_press(ANY_BUTTON);
if(button & BUTTON_A)
play_from_program_space(beep_button_a);
else if(button & BUTTON_B)
play_from_program_space(beep_button_b);
else
play_from_program_space(beep_button_c);
wait_for_button_release(button);
return button;
}
// Initializes the 3pi, displays a welcome message, calibrates, and
// plays the initial music.
void initialize()
{
// 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.
pololu_3pi_init(2000);
load_custom_characters(); // load the custom characters
play_from_program_space(welcome);
print_two_lines_delay_1s(welcome_line1,welcome_line2);
print_two_lines_delay_1s(demo_name_line1,demo_name_line2);
print_two_lines_delay_1s(instructions_line1,instructions_line2);
clear();
print_from_program_space(instructions_line3);
lcd_goto_xy(0,1);
print_from_program_space(instructions_line4);
while(!(wait_for_button_and_beep() & BUTTON_B));
play_from_program_space(thank_you_music);
print_two_lines_delay_1s(thank_you_line1,thank_you_line2);
}
void menu_select()
{
static int menu_index = 0;
print_two_lines_delay_1s(main_menu_intro_line1,main_menu_intro_line2);
while(1)
{
clear();
lcd_goto_xy(0,1);
print_from_program_space(menu_line2);
lcd_goto_xy(0,0);
print_from_program_space(main_menu_options[menu_index]);
lcd_show_cursor(CURSOR_BLINKING);
// the cursor will be blinking at the end of the option name
// wait for all buttons to be released, then a press
while(button_is_pressed(ANY_BUTTON));
char button = wait_for_button_press(ANY_BUTTON);
if(button & BUTTON_A)
{
play_from_program_space(beep_button_a);
menu_index --;
}
else if(button & BUTTON_B)
{
lcd_hide_cursor();
clear();
play_from_program_space(beep_button_b);
wait_for_button_release(button);
while(!button_is_pressed(BUTTON_B))
{
lcd_goto_xy(0,1);
print_from_program_space(back_line2);
lcd_goto_xy(0,0);
main_menu_functions[menu_index]();
}
set_motors(0,0);
stop_playing();
m1_speed = 0;
m2_speed = 0;
red_led(0);
green_led(0);
play_from_program_space(beep_button_b);
return;
}
else if(button & BUTTON_C)
{
play_from_program_space(beep_button_c);
menu_index ++;
}
if(menu_index < 0)
menu_index = main_menu_length-1;
if(menu_index >= main_menu_length)
menu_index = 0;
}
}
// This is the main function, where the code starts. All C programs
// must have a main() function defined somewhere.
int main()
{
// set up the 3pi
initialize();
// This is the "main loop" - it will run forever.
while(1)
{
menu_select();
}
}
// Local Variables: **
// mode: C **
// c-basic-offset: 4 **
// tab-width: 4 **
// indent-tabs-mode: t **
// end: **
Xamran Shamsi
xamran.shamsi@gmail.com
