It’s a kind of subsumption architecture model.
Looks smart like Roomba. But actually, really simple system.
Environment
- Ubuntu 16.04
- Pololu 3pi Robot (ATmega328P)
C code
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#include <pololu/3pi.h> | |
#include <avr/pgmspace.h> | |
#include <stdlib.h> | |
// Introductory messages. The "PROGMEM" identifier causes the data to | |
// go into program space. | |
const char welcome_line1[] PROGMEM = " Pololu"; | |
const char welcome_line2[] PROGMEM = "3\xf7 Robot"; | |
const char demo_name_line1[] PROGMEM = "Line"; | |
const char demo_name_line2[] PROGMEM = "Bouncing"; | |
// A couple of simple tunes, stored in program space. | |
const char welcome[] PROGMEM = ">g32>>c32"; | |
const char go[] PROGMEM = "L16 cdegreg4"; | |
// 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() | |
{ | |
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+3,3); | |
lcd_load_custom_character(levels+4,4); | |
lcd_load_custom_character(levels+5,5); | |
lcd_load_custom_character(levels+6,6); | |
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. | |
print_character(c); | |
} | |
} | |
// Initializes the 3pi, displays a welcome message, calibrates, and | |
// plays the initial music. | |
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()); | |
} | |
int calc_PID(int on_line, int position, int max, int reset){ | |
int proportional = 0; | |
int derivative = 0; | |
static long integral = 0; | |
static unsigned int last_proportional = 0; | |
int power_difference = 0; | |
if (reset){ | |
integral = 0; | |
last_proportional = 0; | |
} | |
// The "proportional" term should be 0 when we are on the line. | |
proportional = (position) - 2000; | |
// Compute the derivative (change) and integral (sum) of the | |
// position. | |
derivative = proportional - last_proportional; | |
integral += proportional; | |
if (!on_line) | |
integral = 0; | |
if (integral > max * 10000) | |
integral = max * 10000; | |
if (integral < -max * 10000) | |
integral = -max * 10000; | |
// 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. | |
power_difference = proportional/10 + integral/10000 + derivative*3/2; | |
// Compute the actual motor settings. We never set either motor | |
// to a negative value. | |
if(power_difference > max) | |
power_difference = max; | |
if(power_difference < -max) | |
power_difference = -max; | |
if (!on_line) | |
power_difference = 0; | |
return power_difference; | |
} | |
// 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 char i, on_line = 0; | |
int power_difference = 0; | |
const int speed_follow = 50; | |
const int speed_rundomw = 60; | |
unsigned long countmax = 0; | |
unsigned long main_loop = 0; | |
unsigned char mode = 0; | |
unsigned char mode_z1 = 0; | |
unsigned char reset_pid = 0; | |
unsigned long main_loop_rundome_start = 0; | |
unsigned long main_loop_rundome_end = 0; | |
// set up the 3pi | |
initialize(); | |
// This is the "main loop" - it will run forever. | |
while(1) | |
{ | |
// mode changer | |
if (main_loop > 3000){ | |
// reset counter | |
main_loop = 0; | |
// mode change | |
if (mode == 0) | |
mode = 1; | |
else if (mode == 1) | |
mode = 0; | |
main_loop_rundome_start = 0; | |
main_loop_rundome_end = 0; | |
} | |
// update sensor | |
unsigned int position = read_line(sensors,IR_EMITTERS_ON); | |
on_line = 0; | |
for(i=0;i<5;i++) { | |
int value = sensors[i]; | |
if(value > 200) { | |
on_line = 1; | |
} | |
} | |
clear(); | |
print_long(mode); | |
print(","); | |
print_long(on_line); | |
print(","); | |
// state machine | |
switch(mode) { | |
// rnudome movement | |
case 0: | |
// drive straight. | |
if (!on_line && main_loop > main_loop_rundome_end){ | |
set_motors(speed_rundomw, speed_rundomw); | |
left_led(1); | |
right_led(1); | |
print(",C"); | |
main_loop_rundome_start = main_loop; | |
countmax = rand()%(200); | |
} | |
else{ | |
main_loop_rundome_end = main_loop_rundome_start + countmax; | |
print_long(countmax); | |
print(","); | |
// turn right. | |
if (countmax%2 == 1){ | |
set_motors(speed_rundomw,-speed_rundomw); | |
left_led(0); | |
right_led(1); | |
print(",R"); | |
} | |
// turn left. | |
else{ | |
set_motors(-speed_rundomw,speed_rundomw); | |
left_led(1); | |
right_led(0); | |
print(",L"); | |
} | |
} | |
break; | |
// follow wall | |
case 1: | |
// drive straight. | |
if (!on_line){ | |
set_motors(speed_follow, speed_follow); | |
left_led(1); | |
right_led(1); | |
print(",C"); | |
} | |
else{ | |
if (mode_z1 != mode) | |
reset_pid = 1; | |
else | |
reset_pid = 0; | |
power_difference = calc_PID(on_line, position, speed_follow, reset_pid); | |
// turn right. | |
if (power_difference < 0){ | |
left_led(0); | |
right_led(1); | |
print(",R"); | |
} | |
// turn left. | |
if (power_difference > 0){ | |
left_led(1); | |
right_led(0); | |
print(",L"); | |
} | |
set_motors(-power_difference, power_difference); | |
print_long(power_difference); | |
} | |
break; | |
default: | |
break; | |
} | |
lcd_goto_xy(0,1); | |
display_readings(sensors); | |
mode_z1 = mode; | |
main_loop += 1; | |
} | |
} |
Result
I assume that it can be a home cleaning robot.