これは Pololu 3pi Robot User’s Guide ≫ 7.b. A Simple Line-Following Algorithm for 3pi の非公式日本語訳です。
目次
前： 7.a. About Line Following
次： 7.c. Advanced Line Following with 3pi: PID Control
7.b. A Simple Line-Following Algorithm for 3pi
A simple line following program for the 3pi is available in the folder examples\atmegaxx8\3pi-linefollower.
3pi用の単純なライン追跡プログラムは examples\atmegaxx8\3pi-linefollower フォルダにあります。
Note: An Arduino-compatible version of this sample program can be downloaded as part of the Pololu Arduino Libraries (see Section 5.g).
注意：このサンプルプログラムのArduino互換バージョンは Pololu Arduino Libraries の一部としてダウンロードできます(Section 5.gを参照)。
The source code demonstrates a variety of different features of the 3pi, including the line sensors, motors, LCD, battery voltage monitor, and buzzer. The program has two phases.
ソースコードは、ラインセンサー、モーター、LCD、電池電圧モニター、ブザーを含む、3piのさまざまな機能のデモンストレーションです。プログラムには2つのフェーズがあります。
The first phase of the program is the initialization and calibration phase, which is handled by the function intitialize(). This function is called once, at the beginning of the main() function, before anything else happens, and it takes care of the following steps:
プログラムの第1フェーズは初期化とキャリブレーションのフェーズで、関数 intitialize() によって処理されます。この関数は関数 main() のはじめで、他の処理が行われる前に1度だけ呼ばれ、以下の手順を実行します：
-
Calling pololu_3pi_init(2000) to set up the 3pi, with the sensor timeout set to 2000×0.4 us = 800 us. This means that the sensor values will vary from 0 (completely white) to 2000 (completely black), where a value of 2000 indicates that the sensor’s capacitor took at least 800 us to discharge.
-
Displaying the battery voltage returned by the read_battery_millivolts() function. It is important to monitor battery voltage so that your robot does not surprisingly run out of batteries and shut down during the middle of a competition or during programming. For more information, see Section 2 of the command reference.
-
Calibrating the sensors. This is accomplished by turning the 3pi to the right and left on the line while calling the calibrate_line_sensors() function. The minimum and maximum values read during this time are stored in RAM. This allows the read_line_sensors_calibrated() function to return values that are adjusted to range from 0 to 1000 for each sensor, even if some of your sensors respond differently than the others. The read_line() function used later in the code also depends on having calibrated values. For more information, see Section 19 of the command reference.
-
Displaying the calibrated line sensor values in a bar graph. This demonstrates the use of the lcd_load_custom_character() function together with print_character() to make it easy to see whether the line sensors are working properly before starting the robot. For more information on this and other LCD commands, see Section 5 of the command reference.
-
Waiting for the user to press a button. It’s very important for your robot not to start driving until you want it to start, or it could unexpectedly drive off of a table or out of your hands when you are trying to program it. We use the button_is_pressed() function to wait for you to press the B button while displaying the battery voltage or sensor readings. For more information on button commands, see Section 9 of the command reference.
-
3piをセットアップするために
pololu_3pi_init(2000)を呼び出します。2000はセンサーのタイムアウト時間で 2000 × 0.4 [us] ＝ 800 [us] です。これはセンサーの値が0(完全な白)から2000(完全な黒)まで変化することを意味します。2000の値は、センサーのコンデンサが放電するのに少なくとも800[us]かかることを示します。 -
read_battery_millivolts()によって返された電池電圧を表示します。ロボットが思いがけず電池を使い果たし、競争の最中または書き込み中にシャットダウンすることがないように電池電圧を監視することは重要です。詳細はcommand referenceのSection 2を参照してください。 -
センサーを較正します。これは
calibrate_line_sensors()を呼び出しながら3piをライン上で左右に回転させることによって達成されます。この間に読み出した最小値と最大値はRAMに保存されます。これによってread_line_sensors_calibrated()は、一部のセンサーが他のセンサーと異なる応答をした場合でも、各センサーについて0～1000の範囲に調整された値を返すことができます。コードの後半で使用されているread_line()も、キャリブレーションされた値を持つことに依存しています。詳細はcommand referenceのSection 19を参照してください。 -
校正後のラインセンサー値を棒グラフで表示します。これは
lcd_load_custom_character()をprint_character()と共に使用して、ロボットをスタートする前にラインセンサーが正しく機能しているかを簡単に確認するデモンストレーションです。これと他のLCDコマンドの詳細はcommand referenceのSection 5を参照してください。 -
ユーザーがボタンを押すのを待ちます。ロボットが、あなたがスタートさせようとするまで駆動を開始しないこと、書き込もうとしているときに不意にテーブルやあなたの手を離れて走り去らないことは非常に重要です。電池電圧やセンサーのリード値を表示している間、Bボタンが押されるのを待つために
button_is_pressed()を使用します。ボタンコマンドの詳細はcommand referenceのSection 9を参照してください。
In the second phase of the program, your 3pi will take a sensor reading and set the motor speed appropriately based on the reading. The general idea is that if the robot is off on either side, it should turn to get back on, but if it’s on the line, it should try to drive straight ahead. The following steps occur inside of a while(1) loop, which will continue repeating over and over until the robot is turned off or reset.
プログラムの第2フェーズは、3piはセンサーをリードし、リード値に基づいてモーターの速度を適切に設定します。ロボットのどちらか片側がオフの場合は再びオンになるべきですが、ライン上にある場合は前方にまっすぐ駆動しようとするべきです。以下の手順はwhile(1)ループの内側で行われ、ロボットの電源が切れるかリセットされるまで繰り返します。
- The function read_line() is called. This takes a sensor reading and returns an estimate of the robot’s position with respect to the line, as a number between 0 and 4000. A value of 0 means that the line is to the left of sensor 0, value of 1000 means that the line is directly under sensor 1, 2000 means that the line is directly under sensor 2, and so on.
- The value returned by read_line() is divided into three possible cases:
- 0–1000: the robot is far to the right of the line. In this case, to turn sharply left, we set the right motor speed to 100 and the left motor speed to 0. Note that the maximum speed of the motors is 255, so we are driving the right motor at only about 40% power here.
- 1000–3000: the robot is approximately centered on the line. In this case, we set both motors to speed 100, to drive straight ahead.
- 3000–4000: the robot is far to the left of the line. In this case, we turn sharply to the right by setting the right motor speed to 0 and the left motor speed to 100.
-
Depending on which motors are activated, the corresponding LEDs are turned on for a more interesting display. This can also help with debugging.
-
read_line()が呼び出されます。これはセンサーのリード値を取得し、ラインに対するロボットの位置の推定を0～4000の間の数値として返します。0のときはラインがセンサー0の左にあることを意味し、1000のときはラインがセンサー1の直下にあることを意味し、2000のときはラインがセンサー2の直下にあることを意味します。 -
read_line()の戻り値は3つのケースに分けられます：
- 0-1000: ロボットはラインのはるか右側にあります。この場合、すばやく左に曲がるために、右のモーターの速度を100に設定し、左のモーターの速度を0に設定します。モーターの最高速度は255であるため、ここでは右のモーターを約40％の電力で駆動しています。
- 1000–3000: ロボットはほぼラインの中央にあります。この場合、直進するために、両方のモーターの速度を100に設定します。
- 3000–4000: ロボットはラインのはるか左側にあります。この場合、すばやく右に曲がるために、右のモーターの速度を0に設定し、左のモーターの速度を100に設定します。
- どのモーターが作動しているかに応じて、対応するLEDが点灯し、より興味深い表示になります。これはデバッグにも役立ちます。
To open the program in Atmel Studio, you may go to examples\atmegaxx8\3pi-linefollower and simply double-click on 3pi-linefollower.cproj. Compile the program, load it onto your 3pi, and try it out. You should find that your robot is able to follow the curves of your line course without ever completely losing the line. However, its motors are moving at a speed of at most 100 out of the maximum possible of 255, and the algorithm causes a lot of unnecessary shaking on the curves. At this point, you might want to work on trying to adjust and improve this algorithm, before moving on to the next section. Some ideas for improvement are:
Atmel Studio でプログラムを開くには、examples\atmegaxx8\3pi-linefollowerを開き、単純に3pi-linefollower.cprojをダブルクリックしてください。プログラムをコンパイルし、3piにロードし、そして試してみてください。ロボットがラインを完全に見失うことなく、ラインコースのカーブを追跡できるところまでたどり着いてください。しかしながら、モーターの動く速度は最大255のうちせいぜい100であり、そしてアルゴリズムはカーブ上で多くの不必要な揺れを引き起こします。この時点で、次のセクションに進む前に、このアルゴリズムの調整と改善を試みることをおすすめします。改善のためのいくつかのアイデアは以下のとおりです：
-
Increase the maximum possible speed.
-
Add more intermediate cases, with intermediate speed settings, to make the motion less jerky.
-
Give your robot a memory: have its maximum speed increase after it has been on the line consistently for a few cycles.
-
最高速度を上げます。
-
動きのぎこちなさを減らすために、ケースの分岐を増やして速度設定を細分化してください。
-
ロボットにメモリを与える：数サイクルにわたり一貫してライン上になった後、最高速度を上昇します。
You might also want to:
あなたは以下のこともできるかもしれません：
-
Measure the speed of your loop, using timing functions from Section 17 of the command reference to time a few thousand cycles or by blinking the LEDs on and off every 1000 cycles.
-
Display sensor readings on the LCD. Since writing to the LCD takes a significant amount of time, you should do this at most few times per second.
-
Incorporate the buzzer into your program. You might want your 3pi to play music while it is driving or make informational beeps that depend on what it is doing. See Section 3 of the command reference for more information on using the buzzer; for music, you’ll want to use the PLAY_CHECK option to avoid disrupting your sensor readings.
-
command referenceのSection 17のタイミング機能を使用して数千サイクルの時間を計るか、1000サイクル毎にLEDをオンオフして点滅させることによって、ループの速度を測定します。
-
LCDにセンサーのリード値を表示します。LCDへの書き込みにはかなりの時間がかかるため、多くても1秒間に数回程度にすべきです。
-
プログラムにブザーを組み込みます。3piが駆動している間音楽を再生するか、またはそれが何をしているかをビープ音として鳴らすことができます。ブザーの使い方の詳細はcommand referenceのSection 3を参照してください。音楽の場合は、PLAY_CHECKオプションを使用してセンサーのリード値を乱さないようにしてください。
The entire source code to this simple line following program is presented below, for your reference.
参考として、単純なライン追跡プログラムのソースコード全体を以下に示します。
/*
* 3pi-linefollower - demo code for the Pololu 3pi Robot
*
* This code will follow a black line on a white background, using a
* very simple algorithm. It demonstrates auto-calibration and use of
* the 3pi IR sensors, motor control, bar graphs using custom
* characters, and music playback, making it a good starting point for
* developing your own more competitive line follower.
*
* http://www.pololu.com/docs/0J21
* http://www.pololu.com
* http://forum.pololu.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
// ATmegaxx8 has 16x more program space than 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 = " Pololu";
const char welcome_line2[] PROGMEM = "3\xf7 Robot";
const char demo_name_line1[] PROGMEM = "Line";
const char demo_name_line2[] PROGMEM = "follower";
// 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());
}
// 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
// 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);
if(position < 1000)
{
// We are far to the right of the line: turn left.
// Set the right motor to 100 and the left motor to zero,
// to do a sharp turn to the left. Note that the maximum
// value of either motor speed is 255, so we are driving
// it at just about 40% of the max.
set_motors(0,100);
// Just for fun, indicate the direction we are turning on
// the LEDs.
left_led(1);
right_led(0);
}
else if(position < 3000)
{
// We are somewhat close to being centered on the line:
// drive straight.
set_motors(100,100);
left_led(1);
right_led(1);
}
else
{
// We are far to the left of the line: turn right.
set_motors(100,0);
left_led(0);
right_led(1);
}
}
// 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);
}