Merge pull request #11 from AutonomyLab/devel

Add API to get light sensor signals

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

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;
+}

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.
        */

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;

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");