diff --git a/CMakeLists.txt b/CMakeLists.txt index 036c026..ce4b9ac 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,13 +22,17 @@ target_link_libraries(create ## Declare example executables add_executable(create_demo examples/create_demo.cpp) +add_executable(bumper_example examples/bumper_example.cpp) ## Specify libraries to link a library or executable target against target_link_libraries(create_demo ${Boost_LIBRARIES} create ) - +target_link_libraries(bumper_example + ${Boost_LIBRARIES} + create +) ## Install install(TARGETS create DESTINATION lib) install(FILES diff --git a/examples/bumper_example.cpp b/examples/bumper_example.cpp new file mode 100644 index 0000000..e6543ee --- /dev/null +++ b/examples/bumper_example.cpp @@ -0,0 +1,73 @@ +#include "create/create.h" + +int main(int argc, char** argv) { + std::string port = "/dev/ttyUSB0"; + int baud = 115200; + create::RobotModel model = create::CREATE_2; + + create::Create* 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); + + uint16_t signals[6]; + bool contact_bumpers[2]; + bool light_bumpers[6]; + + // Stop program when clean button is pressed + while (!robot->isCleanButtonPressed()) { + signals[0] = robot->getLightSignalLeft(); + signals[1] = robot->getLightSignalFrontLeft(); + signals[2] = robot->getLightSignalCenterLeft(); + signals[3] = robot->getLightSignalCenterRight(); + signals[4] = robot->getLightSignalFrontRight(); + signals[5] = robot->getLightSignalRight(); + + contact_bumpers[0] = robot->isLeftBumper(); + contact_bumpers[1] = robot->isRightBumper(); + + light_bumpers[0] = robot->isLightBumperLeft(); + light_bumpers[1] = robot->isLightBumperFrontLeft(); + light_bumpers[2] = robot->isLightBumperCenterLeft(); + light_bumpers[3] = robot->isLightBumperCenterRight(); + light_bumpers[4] = robot->isLightBumperFrontRight(); + light_bumpers[5] = robot->isLightBumperRight(); + + // print signals from left to right + std::cout << "[ " << signals[0] << " , " + << signals[1] << " , " + << signals[2] << " , " + << signals[3] << " , " + << signals[4] << " , " + << signals[5] + << " ]" << std::endl; + std::cout << "[ " << light_bumpers[0] << " , " + << light_bumpers[1] << " , " + << light_bumpers[2] << " , " + << light_bumpers[3] << " , " + << light_bumpers[4] << " , " + << light_bumpers[5] + << " ]" << std::endl; + std::cout << "[ " << contact_bumpers[0] << " , " + << contact_bumpers[1] + << " ]" << std::endl; + std::cout << std::endl; + + usleep(1000 * 100); //10hz + } + + std::cout << "Stopping Create" << std::endl; + + // Disconnect from robot + robot->disconnect(); + delete robot; + + return 0; +} diff --git a/include/create/create.h b/include/create/create.h index 9a4877a..c9c35ea 100644 --- a/include/create/create.h +++ b/include/create/create.h @@ -403,13 +403,35 @@ namespace create { */ bool isLightBumperCenterRight() const; - //TODO (https://github.com/AutonomyLab/libcreate/issues/3) - //uint16_t getDistLeft() const; - //uint16_t getDistFrontLeft() const; - //uint16_t getDistCenterLeft() const; - //uint16_t getDistRight() const; - //uint16_t getDistFrontRight() const; - //uint16_t getDistCenterRight() const; + /* Return the signal strength from the left light sensor. + * \return value in range [0, 4095] + */ + uint16_t getLightSignalLeft() const; + + /* Return the signal strength from the front-left light sensor. + * \return value in range [0, 4095] + */ + uint16_t getLightSignalFrontLeft() const; + + /* Return the signal strength from the center-left light sensor. + * \return value in range [0, 4095] + */ + uint16_t getLightSignalCenterLeft() const; + + /* Return the signal strength from the right light sensor. + * \return value in range [0, 4095] + */ + uint16_t getLightSignalRight() const; + + /* Return the signal strength from the front-right light sensor. + * \return value in range [0, 4095] + */ + uint16_t getLightSignalFrontRight() const; + + /* Return the signal strength from the center-right light sensor. + * \return value in range [0, 4095] + */ + uint16_t getLightSignalCenterRight() const; /* Return true if Create is moving forward, false otherwise. */ diff --git a/src/create.cpp b/src/create.cpp index b0cc473..7a7a99e 100644 --- a/src/create.cpp +++ b/src/create.cpp @@ -677,6 +677,66 @@ namespace create { } } + uint16_t Create::getLightSignalLeft() const { + if (data->isValidPacketID(ID_LIGHT_LEFT)) { + return GET_DATA(ID_LIGHT_LEFT); + } + else { + CERR("[create::Create] ", "Light sensors not supported!"); + return 0; + } + } + + uint16_t Create::getLightSignalFrontLeft() const { + if (data->isValidPacketID(ID_LIGHT_FRONT_LEFT)) { + return GET_DATA(ID_LIGHT_FRONT_LEFT); + } + else { + CERR("[create::Create] ", "Light sensors not supported!"); + return 0; + } + } + + uint16_t Create::getLightSignalCenterLeft() const { + if (data->isValidPacketID(ID_LIGHT_CENTER_LEFT)) { + return GET_DATA(ID_LIGHT_CENTER_LEFT); + } + else { + CERR("[create::Create] ", "Light sensors not supported!"); + return 0; + } + } + + uint16_t Create::getLightSignalRight() const { + if (data->isValidPacketID(ID_LIGHT_RIGHT)) { + return GET_DATA(ID_LIGHT_RIGHT); + } + else { + CERR("[create::Create] ", "Light sensors not supported!"); + return 0; + } + } + + uint16_t Create::getLightSignalFrontRight() const { + if (data->isValidPacketID(ID_LIGHT_FRONT_RIGHT)) { + return GET_DATA(ID_LIGHT_FRONT_RIGHT); + } + else { + CERR("[create::Create] ", "Light sensors not supported!"); + return 0; + } + } + + uint16_t Create::getLightSignalCenterRight() const { + if (data->isValidPacketID(ID_LIGHT_CENTER_RIGHT)) { + return GET_DATA(ID_LIGHT_CENTER_RIGHT); + } + else { + CERR("[create::Create] ", "Light sensors not supported!"); + return 0; + } + } + bool Create::isMovingForward() const { if (data->isValidPacketID(ID_STASIS)) { return GET_DATA(ID_STASIS) == 1; diff --git a/src/data.cpp b/src/data.cpp index fe6dc29..4960fed 100644 --- a/src/data.cpp +++ b/src/data.cpp @@ -53,12 +53,12 @@ namespace create { ADD_PACKET(ID_LEFT_ENC, 2, "enc_counts_left"); ADD_PACKET(ID_RIGHT_ENC, 2, "enc_counts_right"); ADD_PACKET(ID_LIGHT, 1, "light_bumper"); - //ADD_PACKET(ID_LIGHT_LEFT, 2, "light_bumper_left"); - //ADD_PACKET(ID_LIGHT_FRONT_LEFT, 2, "light_bumper_front_left"); - //ADD_PACKET(ID_LIGHT_CENTER_LEFT, 2, "light_bumper_center_left"); - //ADD_PACKET(ID_LIGHT_CENTER_RIGHT, 2, "light_bumper_center_right"); - //ADD_PACKET(ID_LIGHT_FRONT_RIGHT, 2, "light_bumper_front_right"); - //ADD_PACKET(ID_LIGHT_RIGHT, 2, "light_bumper_right"); + ADD_PACKET(ID_LIGHT_LEFT, 2, "light_bumper_left"); + ADD_PACKET(ID_LIGHT_FRONT_LEFT, 2, "light_bumper_front_left"); + ADD_PACKET(ID_LIGHT_CENTER_LEFT, 2, "light_bumper_center_left"); + ADD_PACKET(ID_LIGHT_CENTER_RIGHT, 2, "light_bumper_center_right"); + ADD_PACKET(ID_LIGHT_FRONT_RIGHT, 2, "light_bumper_front_right"); + ADD_PACKET(ID_LIGHT_RIGHT, 2, "light_bumper_right"); ADD_PACKET(ID_IR_LEFT, 1, "ir_opcode_left"); ADD_PACKET(ID_IR_RIGHT, 1, "ir_opcode_right"); //ADD_PACKET(ID_LEFT_MOTOR_CURRENT, 2, "left_motor_current");