print_position.cpp

移動しながら自己位置を表示する

#include <iostream>
#include "Roomba_driver.h"
#include "robot_utils.h"
#include "print_mm_position.h"
#include "ticks.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;
    }

    // 移動の開始
    robot.follow_line(PositionF(0, 0, deg(0)));

    // しばらく位置を表示してから停止させる
    const double Print_period_sec = 2.0;
    const double first_sec = ticks_sec();
    while ((ticks_sec() - first_sec) < Print_period_sec) {
        print_mm_position(robot.position());
        delay_sec(0.1);
    }

    robot.stop();
    wait_stable(robot, 0.1);

    print_mm_position(robot.position());

    return 0;
}