50 lines
1.1 KiB
C++
50 lines
1.1 KiB
C++
#include "create/create.h"
|
|
|
|
create::Create* robot;
|
|
|
|
int main(int argc, char** argv) {
|
|
std::string port = "/dev/ttyUSB0";
|
|
int baud = 115200;
|
|
create::RobotModel model = create::CREATE_2;
|
|
|
|
if (argc > 1 && std::string(argv[1]) == "create1") {
|
|
model = create::CREATE_1;
|
|
baud = 57600;
|
|
std::cout << "1st generation Create selected" << std::endl;
|
|
}
|
|
|
|
robot = new create::Create(model);
|
|
|
|
// Attempt to connect to Create
|
|
if (robot->connect(port, baud))
|
|
std::cout << "Successfully connected to Create" << std::endl;
|
|
else {
|
|
std::cout << "Failed to connect to Create on port " << port.c_str() << std::endl;
|
|
return 1;
|
|
}
|
|
|
|
robot->setMode(create::MODE_FULL);
|
|
|
|
usleep(1000000);
|
|
|
|
// drive in a circle
|
|
robot->drive(0.1, 0.5);
|
|
|
|
// Quit when center "Clean" button pressed
|
|
while (!robot->isCleanButtonPressed()) {
|
|
create::Pose pose = robot->getPose();
|
|
|
|
std::cout << "x: " << pose.x
|
|
<< "\ty: " << pose.y
|
|
<< "\tyaw: " << pose.yaw * 180.0/create::util::PI << std::endl;
|
|
|
|
usleep(1000 * 100); //10hz
|
|
}
|
|
|
|
std::cout << "Stopping Create." << std::endl;
|
|
|
|
robot->disconnect();
|
|
delete robot;
|
|
|
|
return 0;
|
|
}
|