bumpers.cpp

Go to the documentation of this file.

#include "create/create.h"

#include <iostream>

int main(int argc, char** argv) {
  // Select robot. Assume Create 2 unless argument says otherwise
  create::RobotModel model = create::RobotModel::CREATE_2;
  std::string port = "/dev/ttyUSB0";
  int baud = 115200;
  if (argc > 1 && std::string(argv[1]) == "create1") {
    model = create::RobotModel::CREATE_1;
    baud = 57600;
    std::cout << "Running driver for Create 1" << std::endl;
  }
  else {
    std::cout << "Running driver for Create 2" << std::endl;
  }

  // Construct robot object
  create::Create robot(model);

  // Connect to robot
  if (robot.connect(port, baud))
    std::cout << "Connected to robot" << std::endl;
  else {
    std::cout << "Failed to connect to robot on port " << port.c_str() << std::endl;
    return 1;
  }

  // Switch to Full mode
  robot.setMode(create::MODE_FULL);

  uint16_t light_signals[6] = {0, 0, 0, 0, 0, 0};
  bool light_bumpers[6] = {false, false, false, false, false, false};
  bool contact_bumpers[2] = {false, false};

  while (true) {
    // Get light sensor data (only available for Create 2 or later robots)
    if (model == create::RobotModel::CREATE_2) {
      // Get raw light signal values
      light_signals[0] = robot.getLightSignalLeft();
      light_signals[1] = robot.getLightSignalFrontLeft();
      light_signals[2] = robot.getLightSignalCenterLeft();
      light_signals[3] = robot.getLightSignalCenterRight();
      light_signals[4] = robot.getLightSignalFrontRight();
      light_signals[5] = robot.getLightSignalRight();

      // Get obstacle data from light sensors (true/false)
      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();
    }

    // Get state of bumpers
    contact_bumpers[0] = robot.isLeftBumper();
    contact_bumpers[1] = robot.isRightBumper();

    // Print signals from left to right
    std::cout << "[ " << light_signals[0] << " , "
                      << light_signals[1] << " , "
                      << light_signals[2] << " , "
                      << light_signals[3] << " , "
                      << light_signals[4] << " , "
                      << light_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(100000);  // 10 Hz
  }

  return 0;
}