follow_lines_stop.cpp

一旦停止してから次の直線を走行する

#include <cstdlib>
#include <iostream>
#include "Roomba_driver.h"
#include "robot_utils.h"
#include "delay.h"
#include "log_printf.h"


using namespace hrk;
using namespace std;


int main(int argc, char *argv[])
{
    static_cast<void>(argc);
    static_cast<void>(argv);

    Roomba_driver robot;
    if (!robot.open("/dev/ttyUSB0")) {
        cerr << FILE_POSITION() << robot.what() << endl;
        return 1;
    }

    // 移動の開始
    // 直線の点を最後の位置(1.0, 0.0)で定義してあるのは、
    // 残りの距離を計算するときに便利なため。
    const PositionF first_line(1.0, 0.0, deg(0));
    robot.follow_line(first_line);

    // 次の直線に近付くまでは移動させ続ける
    const PositionF next_line(1.0, 0.0, deg(90));
    const double Stop_distance = 0.3; // 次の直線への停止を開始する距離
    while (robot.distance_to_perpendicular(first_line) < -Stop_distance) {
        delay_sec(0.1);
    }

    // 直線上に停止させる
    robot.stop_to_line(first_line);
    wait_stable(robot, 0.1);

    // 90 度だけ転回
    robot.spin_to_direction(deg(90));
    wait_stable(robot, 0.1);

    // 次の直線追従
    robot.follow_line(next_line);

    // 停止させて終了する
    delay_sec(3.0);
    robot.stop();
    wait_stable(robot, 0.1);

    return 0;
}