Test application to control icarbot through btle.

icarbot_test.cpp

/*
 *
 *  ROS iCarbot driver
 *
 *  Copyright (C) 2017  Kenta Yonekura <[email protected]>
 *
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 */

// $ g++ -o icarbot_test icarbot_test.cpp -I/usr/include/glib-2.0/ -I/usr/lib/x86_64-linux-gnu/glib-2.0/include/ -lgattlib -lglib-2.0

#include <assert.h>
#include <glib.h>
#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <unistd.h> // for sleep

#include <gattlib.h>

static uuid_t g_uuid, g_uuid_note;
long int value_data;

void notification_handler(const uuid_t* uuid, const uint8_t* data, size_t data_length, void* user_data) {
  int i;

  std::cout << "Notification Handler: ";

  for (i = 0; i < data_length; i++) {
    printf("%02x ", data[i]);
  }
  printf("\n");
}


int main(int argc, char *argv[]) {
  int i, ret;
  size_t len;
  char *uuid = "0000ffe1-0000-1000-8000-00805f9b34fb";

  unsigned char forward[] = {0x55, 0xaa, 0x0b, 0x20, 0x00, 0x7f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x4c, 0x36};
  unsigned char backward[] = {0x55, 0xaa, 0x0b, 0x20, 0x00, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xe9, 0x47};
  unsigned char turn_right[] = {0x55, 0xaa, 0x0b, 0x20, 0x00, 0x00, 0x7f, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc2, 0x21};
  unsigned char turn_left[] = {0x55, 0xaa, 0x0b, 0x20, 0x00, 0x00, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x96, 0x9e};

  unsigned char get_status0[] = {0x55, 0xaa, 0x0b, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0e, 0x61};
  unsigned char get_status1[] = {0x55, 0xaa, 0x0b, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x23, 0x25};
  unsigned char get_status2[] = {0x55, 0xaa, 0x0b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xe5, 0x42};

  gatt_connection_t* connection;

  if (argc != 2) {
    std::cout << "Usage: ./icarbot [BTLE_ADDR](e.g. 88:4A:EA:8B:F4:01)" << std::endl;
    return 1;
  }

  // convert uuid name to uuid
  if (gattlib_string_to_uuid(uuid, strlen(uuid) + 1, &g_uuid) < 0) {
    std::cout << "UUID is not correct." << std::endl;
    return 1;
  }

  connection = gattlib_connect(NULL, argv[1], GATTLIB_CONNECTION_OPTIONS_LEGACY_DEFAULT);
  if (connection == NULL) {
    std::cout << "Fail to connect to the bluetooth device." << std::endl;
    return 1;
  }

  // subscribe notification
  gattlib_register_notification(connection, notification_handler, NULL);

  ret = gattlib_notification_start(connection, &g_uuid);
  if (ret) {
    fprintf(stderr, "Fail to start notification\n.");
    return 1;
  }

  // main loop
  while(1){
/*    ret = gattlib_write_char_by_uuid(connection, &g_uuid, get_status0, sizeof(get_status0));
    assert(ret == 0);
    ret = gattlib_write_char_by_uuid(connection, &g_uuid, get_status1, sizeof(get_status1));
    assert(ret == 0);
    ret = gattlib_write_char_by_uuid(connection, &g_uuid, get_status2, sizeof(get_status2));
    assert(ret == 0);
*/
    ret = gattlib_write_char_by_uuid(connection, &g_uuid, forward, sizeof(forward));
    assert(ret == 0);
    g_main_context_iteration(NULL, 1);
    sleep(1);

    ret = gattlib_write_char_by_uuid(connection, &g_uuid, turn_right, sizeof(turn_right));
    assert(ret == 0);
    g_main_context_iteration(NULL, 1);
    sleep(1.5);
  }

  gattlib_disconnect(connection);

  return 0;
}