leds.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);

  bool enable_check_robot_led = true;
  bool enable_debris_led = false;
  bool enable_dock_led = true;
  bool enable_spot_led = false;
  uint8_t power_led = 0;
  char digit_buffer[5];

  while (true) {
    // Set LEDs
    robot.enableCheckRobotLED(enable_check_robot_led);
    robot.enableDebrisLED(enable_debris_led);
    robot.enableDockLED(enable_dock_led);
    robot.enableSpotLED(enable_spot_led);
    robot.setPowerLED(power_led);

    // Set 7-segment displays
    const int len = sprintf(digit_buffer, "%d", power_led);
    for (int i = len; i < 4; i++) digit_buffer[i] = ' ';
    robot.setDigitsASCII(digit_buffer[0], digit_buffer[1], digit_buffer[2], digit_buffer[3]);

    // Update LED values
    enable_check_robot_led = !enable_check_robot_led;
    enable_debris_led = !enable_debris_led;
    enable_dock_led = !enable_dock_led;
    enable_spot_led = !enable_spot_led;
    power_led++;

    usleep(250000);  // 5 Hz
  }

  return 0;
}