Added driver and fixed it

2025-08-04 12:24:57 +00:00 · 2024-09-18 16:41:44 +02:00
parent 0bc6743adf
commit 55826ea759
12 changed files with 1405 additions and 1 deletions
--- a/src/C++/Driver/.gitignore
+++ b/src/C++/Driver/.gitignore
@@ -0,0 +1,33 @@
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+*.smod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
+
+CMakeFiles/*
+build/obj/*
+
--- a/src/C++/Driver/CMakeLists.txt
+++ b/src/C++/Driver/CMakeLists.txt
@@ -0,0 +1,13 @@
+cmake_minimum_required(VERSION 3.9)
+project(kobuki_control)
+
+#set(CMAKE_CXX_STANDARD 11)
+find_package( OpenCV REQUIRED )
+
+set(SOURCE_FILES
+        src/CKobuki.cpp
+        src/CKobuki.h
+        src/main.cpp)
+
+add_executable(kobuki_control ${SOURCE_FILES})
+#target_link_libraries(kobuki_control )
--- a/src/C++/Driver/LICENSE
+++ b/src/C++/Driver/LICENSE
@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2016 National Centre of Robotics
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
--- a/src/C++/Driver/Makefile
+++ b/src/C++/Driver/Makefile
@@ -0,0 +1,208 @@
+# CMAKE generated file: DO NOT EDIT!
+# Generated by "Unix Makefiles" Generator, CMake Version 3.30
+
+# Default target executed when no arguments are given to make.
+default_target: all
+.PHONY : default_target
+
+# Allow only one "make -f Makefile2" at a time, but pass parallelism.
+.NOTPARALLEL:
+
+#=============================================================================
+# Special targets provided by cmake.
+
+# Disable implicit rules so canonical targets will work.
+.SUFFIXES:
+
+# Disable VCS-based implicit rules.
+% : %,v
+
+# Disable VCS-based implicit rules.
+% : RCS/%
+
+# Disable VCS-based implicit rules.
+% : RCS/%,v
+
+# Disable VCS-based implicit rules.
+% : SCCS/s.%
+
+# Disable VCS-based implicit rules.
+% : s.%
+
+.SUFFIXES: .hpux_make_needs_suffix_list
+
+# Command-line flag to silence nested $(MAKE).
+$(VERBOSE)MAKESILENT = -s
+
+#Suppress display of executed commands.
+$(VERBOSE).SILENT:
+
+# A target that is always out of date.
+cmake_force:
+.PHONY : cmake_force
+
+#=============================================================================
+# Set environment variables for the build.
+
+# The shell in which to execute make rules.
+SHELL = /bin/sh
+
+# The CMake executable.
+CMAKE_COMMAND = /usr/bin/cmake
+
+# The command to remove a file.
+RM = /usr/bin/cmake -E rm -f
+
+# Escaping for special characters.
+EQUALS = =
+
+# The top-level source directory on which CMake was run.
+CMAKE_SOURCE_DIR = /home/sam/Desktop/rooziinuubii79/src/C++/Driver
+
+# The top-level build directory on which CMake was run.
+CMAKE_BINARY_DIR = /home/sam/Desktop/rooziinuubii79/src/C++/Driver
+
+#=============================================================================
+# Targets provided globally by CMake.
+
+# Special rule for the target edit_cache
+edit_cache:
+	@$(CMAKE_COMMAND) -E cmake_echo_color "--switch=$(COLOR)" --cyan "Running CMake cache editor..."
+	/usr/bin/ccmake -S$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR)
+.PHONY : edit_cache
+
+# Special rule for the target edit_cache
+edit_cache/fast: edit_cache
+.PHONY : edit_cache/fast
+
+# Special rule for the target rebuild_cache
+rebuild_cache:
+	@$(CMAKE_COMMAND) -E cmake_echo_color "--switch=$(COLOR)" --cyan "Running CMake to regenerate build system..."
+	/usr/bin/cmake --regenerate-during-build -S$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR)
+.PHONY : rebuild_cache
+
+# Special rule for the target rebuild_cache
+rebuild_cache/fast: rebuild_cache
+.PHONY : rebuild_cache/fast
+
+# The main all target
+all: cmake_check_build_system
+	$(CMAKE_COMMAND) -E cmake_progress_start /home/sam/Desktop/rooziinuubii79/src/C++/Driver/CMakeFiles /home/sam/Desktop/rooziinuubii79/src/C++/Driver//CMakeFiles/progress.marks
+	$(MAKE) $(MAKESILENT) -f CMakeFiles/Makefile2 all
+	$(CMAKE_COMMAND) -E cmake_progress_start /home/sam/Desktop/rooziinuubii79/src/C++/Driver/CMakeFiles 0
+.PHONY : all
+
+# The main clean target
+clean:
+	$(MAKE) $(MAKESILENT) -f CMakeFiles/Makefile2 clean
+.PHONY : clean
+
+# The main clean target
+clean/fast: clean
+.PHONY : clean/fast
+
+# Prepare targets for installation.
+preinstall: all
+	$(MAKE) $(MAKESILENT) -f CMakeFiles/Makefile2 preinstall
+.PHONY : preinstall
+
+# Prepare targets for installation.
+preinstall/fast:
+	$(MAKE) $(MAKESILENT) -f CMakeFiles/Makefile2 preinstall
+.PHONY : preinstall/fast
+
+# clear depends
+depend:
+	$(CMAKE_COMMAND) -S$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 1
+.PHONY : depend
+
+#=============================================================================
+# Target rules for targets named kobuki_control
+
+# Build rule for target.
+kobuki_control: cmake_check_build_system
+	$(MAKE) $(MAKESILENT) -f CMakeFiles/Makefile2 kobuki_control
+.PHONY : kobuki_control
+
+# fast build rule for target.
+kobuki_control/fast:
+	$(MAKE) $(MAKESILENT) -f CMakeFiles/kobuki_control.dir/build.make CMakeFiles/kobuki_control.dir/build
+.PHONY : kobuki_control/fast
+
+src/CKobuki.o: src/CKobuki.cpp.o
+.PHONY : src/CKobuki.o
+
+# target to build an object file
+src/CKobuki.cpp.o:
+	$(MAKE) $(MAKESILENT) -f CMakeFiles/kobuki_control.dir/build.make CMakeFiles/kobuki_control.dir/src/CKobuki.cpp.o
+.PHONY : src/CKobuki.cpp.o
+
+src/CKobuki.i: src/CKobuki.cpp.i
+.PHONY : src/CKobuki.i
+
+# target to preprocess a source file
+src/CKobuki.cpp.i:
+	$(MAKE) $(MAKESILENT) -f CMakeFiles/kobuki_control.dir/build.make CMakeFiles/kobuki_control.dir/src/CKobuki.cpp.i
+.PHONY : src/CKobuki.cpp.i
+
+src/CKobuki.s: src/CKobuki.cpp.s
+.PHONY : src/CKobuki.s
+
+# target to generate assembly for a file
+src/CKobuki.cpp.s:
+	$(MAKE) $(MAKESILENT) -f CMakeFiles/kobuki_control.dir/build.make CMakeFiles/kobuki_control.dir/src/CKobuki.cpp.s
+.PHONY : src/CKobuki.cpp.s
+
+src/main.o: src/main.cpp.o
+.PHONY : src/main.o
+
+# target to build an object file
+src/main.cpp.o:
+	$(MAKE) $(MAKESILENT) -f CMakeFiles/kobuki_control.dir/build.make CMakeFiles/kobuki_control.dir/src/main.cpp.o
+.PHONY : src/main.cpp.o
+
+src/main.i: src/main.cpp.i
+.PHONY : src/main.i
+
+# target to preprocess a source file
+src/main.cpp.i:
+	$(MAKE) $(MAKESILENT) -f CMakeFiles/kobuki_control.dir/build.make CMakeFiles/kobuki_control.dir/src/main.cpp.i
+.PHONY : src/main.cpp.i
+
+src/main.s: src/main.cpp.s
+.PHONY : src/main.s
+
+# target to generate assembly for a file
+src/main.cpp.s:
+	$(MAKE) $(MAKESILENT) -f CMakeFiles/kobuki_control.dir/build.make CMakeFiles/kobuki_control.dir/src/main.cpp.s
+.PHONY : src/main.cpp.s
+
+# Help Target
+help:
+	@echo "The following are some of the valid targets for this Makefile:"
+	@echo "... all (the default if no target is provided)"
+	@echo "... clean"
+	@echo "... depend"
+	@echo "... edit_cache"
+	@echo "... rebuild_cache"
+	@echo "... kobuki_control"
+	@echo "... src/CKobuki.o"
+	@echo "... src/CKobuki.i"
+	@echo "... src/CKobuki.s"
+	@echo "... src/main.o"
+	@echo "... src/main.i"
+	@echo "... src/main.s"
+.PHONY : help
+
+
+
+#=============================================================================
+# Special targets to cleanup operation of make.
+
+# Special rule to run CMake to check the build system integrity.
+# No rule that depends on this can have commands that come from listfiles
+# because they might be regenerated.
+cmake_check_build_system:
+	$(CMAKE_COMMAND) -S$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0
+.PHONY : cmake_check_build_system
+
--- a/src/C++/Driver/cmake_install.cmake
+++ b/src/C++/Driver/cmake_install.cmake
@@ -0,0 +1,62 @@
+# Install script for directory: /home/sam/Desktop/rooziinuubii79/src/C++/Driver
+
+# Set the install prefix
+if(NOT DEFINED CMAKE_INSTALL_PREFIX)
+  set(CMAKE_INSTALL_PREFIX "/usr/local")
+endif()
+string(REGEX REPLACE "/$" "" CMAKE_INSTALL_PREFIX "${CMAKE_INSTALL_PREFIX}")
+
+# Set the install configuration name.
+if(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME)
+  if(BUILD_TYPE)
+    string(REGEX REPLACE "^[^A-Za-z0-9_]+" ""
+           CMAKE_INSTALL_CONFIG_NAME "${BUILD_TYPE}")
+  else()
+    set(CMAKE_INSTALL_CONFIG_NAME "")
+  endif()
+  message(STATUS "Install configuration: \"${CMAKE_INSTALL_CONFIG_NAME}\"")
+endif()
+
+# Set the component getting installed.
+if(NOT CMAKE_INSTALL_COMPONENT)
+  if(COMPONENT)
+    message(STATUS "Install component: \"${COMPONENT}\"")
+    set(CMAKE_INSTALL_COMPONENT "${COMPONENT}")
+  else()
+    set(CMAKE_INSTALL_COMPONENT)
+  endif()
+endif()
+
+# Install shared libraries without execute permission?
+if(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE)
+  set(CMAKE_INSTALL_SO_NO_EXE "0")
+endif()
+
+# Is this installation the result of a crosscompile?
+if(NOT DEFINED CMAKE_CROSSCOMPILING)
+  set(CMAKE_CROSSCOMPILING "FALSE")
+endif()
+
+# Set path to fallback-tool for dependency-resolution.
+if(NOT DEFINED CMAKE_OBJDUMP)
+  set(CMAKE_OBJDUMP "/usr/bin/objdump")
+endif()
+
+if(CMAKE_INSTALL_COMPONENT)
+  if(CMAKE_INSTALL_COMPONENT MATCHES "^[a-zA-Z0-9_.+-]+$")
+    set(CMAKE_INSTALL_MANIFEST "install_manifest_${CMAKE_INSTALL_COMPONENT}.txt")
+  else()
+    string(MD5 CMAKE_INST_COMP_HASH "${CMAKE_INSTALL_COMPONENT}")
+    set(CMAKE_INSTALL_MANIFEST "install_manifest_${CMAKE_INST_COMP_HASH}.txt")
+    unset(CMAKE_INST_COMP_HASH)
+  endif()
+else()
+  set(CMAKE_INSTALL_MANIFEST "install_manifest.txt")
+endif()
+
+if(NOT CMAKE_INSTALL_LOCAL_ONLY)
+  string(REPLACE ";" "\n" CMAKE_INSTALL_MANIFEST_CONTENT
+       "${CMAKE_INSTALL_MANIFEST_FILES}")
+  file(WRITE "/home/sam/Desktop/rooziinuubii79/src/C++/Driver/${CMAKE_INSTALL_MANIFEST}"
+     "${CMAKE_INSTALL_MANIFEST_CONTENT}")
+endif()
--- a/src/C++/Driver/src/CKobuki.cpp
+++ b/src/C++/Driver/src/CKobuki.cpp
@@ -0,0 +1,740 @@
+#include "CKobuki.h"
+#include "termios.h"
+#include "errno.h"
+#include <cstddef>
+#include <iostream>
+
+plot p;
+static std::vector<float> vectorX;
+static std::vector<float> vectorY;
+static std::vector<float> vectorGyroTheta;
+
+// obsluha tty pod unixom
+int set_interface_attribs2 (int fd, int speed, int parity)
+{
+    struct termios tty;
+    memset (&tty, 0, sizeof tty);
+    if (tcgetattr (fd, &tty) != 0)
+    {
+        printf ("error %d from tcgetattr", errno);
+        return -1;
+    }
+
+    cfsetospeed (&tty, speed);
+    cfsetispeed (&tty, speed);
+
+    tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8;     // 8-bit chars
+    // disable IGNBRK for mismatched speed tests; otherwise receive break
+    // as \000 chars
+    //tty.c_iflag &= ~IGNBRK;         // disable break processing
+    tty.c_lflag = 0;                // no signaling chars, no echo,
+    // no canonical processing
+    tty.c_oflag = 0;                // no remapping, no delays
+    tty.c_cc[VMIN]  = 0;            // read doesn't block
+    tty.c_cc[VTIME] = 5;            // 0.5 seconds read timeout
+
+    tty.c_iflag &= ~(IGNBRK | INLCR | ICRNL | IXON | IXOFF | IXANY); // shut off xon/xoff ctrl
+
+    tty.c_cflag |= (CLOCAL | CREAD);// ignore modem controls,
+    // enable reading
+    tty.c_cflag &= ~(PARENB | PARODD);      // shut off parity
+    tty.c_cflag |= parity;
+    tty.c_cflag &= ~CSTOPB;
+    tty.c_cflag &= ~CRTSCTS;
+
+    if (tcsetattr (fd, TCSANOW, &tty) != 0)
+    {
+        printf ("error %d from tcsetattr", errno);
+        return -1;
+    }
+    return 0;
+}
+
+void set_blocking2 (int fd, int should_block)
+{
+    struct termios tty;
+    memset (&tty, 0, sizeof tty);
+    if (tcgetattr (fd, &tty) != 0)
+    {
+        printf ("error %d from tggetattr", errno);
+        return;
+    }
+
+    tty.c_cc[VMIN]  = should_block ? 1 : 0;
+    tty.c_cc[VTIME] = 5;            // 0.5 seconds read timeout
+
+    if (tcsetattr (fd, TCSANOW, &tty) != 0)
+        printf ("error %d setting term attributes", errno);
+}
+
+
+
+
+
+
+int CKobuki::connect(char * comportT)
+{
+    HCom= open(comportT,O_RDWR|O_NOCTTY|O_NONBLOCK);
+
+    if ( HCom== -1 )
+    {
+        printf("Kobuki nepripojeny\n");
+        return HCom;
+
+    }
+    else
+    {
+        set_interface_attribs2 (HCom, B115200, 0);  // set speed to 115,200 bps, 8n1 (no parity)
+        set_blocking2 (HCom, 0);                // set no blocking
+        /*  struct termios settings;
+          tcgetattr(HCom, &settings);
+
+          cfsetospeed(&settings, B115200); // baud rate
+          settings.c_cflag &= ~PARENB; // no parity
+          settings.c_cflag &= ~CSTOPB; // 1 stop bit
+          settings.c_cflag &= ~CSIZE;
+          settings.c_cflag |= CS8 | CLOCAL; // 8 bits
+          settings.c_lflag &= ~ICANON; // canonical mode
+          settings.c_cc[VTIME]=1;
+          settings.c_oflag &= ~OPOST; // raw output
+
+          tcsetattr(HCom, TCSANOW, &settings); // apply the settings*/
+        tcflush(HCom, TCOFLUSH);
+
+
+        printf("Kobuki pripojeny\n");
+        return HCom;
+    }
+}
+
+unsigned char * CKobuki::readKobukiMessage()
+{
+    unsigned char buffer[1];
+    ssize_t Pocet;
+    buffer[0] = 0;
+    unsigned char * null_buffer(0);
+    //citame kym nezachytime zaciatok spravy
+    do {
+        Pocet=read(HCom,buffer,1);
+    } while (buffer[0] != 0xAA);
+    //mame zaciatok spravy (asi)
+    if (Pocet == 1 && buffer[0] == 0xAA)
+    {
+        //citame dalsi byte
+        do {
+
+            Pocet=read(HCom,buffer,1);
+
+        } while (Pocet != 1); // na linuxe -1 na windowse 0
+
+
+
+        //a ak je to druhy byte hlavicky
+        if (Pocet == 1 && buffer[0] == 0x55)
+        {
+            // precitame dlzku
+            Pocet=read(HCom,buffer,1);
+
+            // ReadFile(hCom, buffer, 1, &Pocet, NULL);
+            if (Pocet == 1)
+            {
+                //mame dlzku.. nastavime vektor a precitame ho cely
+                int readLenght = buffer[0];
+                unsigned char *outputBuffer = (unsigned char*)calloc(readLenght+4,sizeof(char));
+                outputBuffer[0] = buffer[0];
+                int pct = 0;
+
+                do
+                {
+                    Pocet = 0;
+                    int readpoc = (readLenght+1  - pct);
+                    Pocet=read(HCom,outputBuffer+1+pct,readpoc);
+
+                    pct = pct + (Pocet == -1 ? 0 : Pocet);
+                } while (pct != (readLenght+1 ));
+
+                // tu si mozeme ceknut co chodi zo serial intefejsu Kobukiho
+                // for(int i=0;i<outputBuffer[0]+2;i++)
+                // {
+                // 	printf("%x ",outputBuffer[i]);
+                // }
+
+                return outputBuffer;
+            }
+        }
+    }
+
+    return null_buffer;
+}
+
+int CKobuki::checkChecksum(unsigned char * data)
+{//najprv hlavicku
+    unsigned char chckSum = 0;
+    for (int i = 0; i < data[0]+2; i++)
+    {
+        chckSum ^= data[i];
+    }
+    return chckSum;//0 ak je vsetko v poriadku,inak nejake cislo
+}
+
+void CKobuki::setLed(int led1, int led2)
+{
+    unsigned char message[8] = {0xaa,0x55,0x04,0x0c,0x02,0x00,(unsigned char)((led1+led2*4)%256),0x00};
+    message[7] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6];
+    uint32_t pocet;
+    pocet=write(HCom,&message,8);
+}
+
+// tato funkcia nema moc sama o sebe vyznam, payload o tom, ze maju byt externe napajania aktivne musi byt aj tak v kazdej sprave...
+void CKobuki::setPower(int value){
+    if (value == 1) {
+        unsigned char message[8] = {0xaa,0x55,0x04,0x0C,0x02,0xf0,0x00,0xAF};
+        uint32_t pocet;
+        pocet = write(HCom,&message,8);
+    }
+}
+
+
+void CKobuki::setTranslationSpeed(int mmpersec)
+{
+    unsigned char message[14] = { 0xaa,0x55,0x0A,0x0c,0x02,0xf0,0x00,0x01,0x04,mmpersec%256,mmpersec>>8,0x00,0x00,  0x00 };
+    message[13] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^ message[7] ^ message[8] ^ message[9] ^ message[10] ^ message[11] ^ message[12];
+
+    uint32_t pocet;
+    pocet=write(HCom,&message,14);
+
+}
+
+void CKobuki::setRotationSpeed(double radpersec)
+{
+    int speedvalue = radpersec * 230.0f / 2.0f;
+    unsigned char message[14] = { 0xaa,0x55,0x0A,0x0c,0x02,0xf0,0x00,0x01,0x04,speedvalue % 256,speedvalue >>8,0x01,0x00,  0x00 };
+    message[13] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^ message[7] ^ message[8] ^ message[9] ^ message[10] ^ message[11] ^ message[12];
+
+    uint32_t pocet;
+    pocet=write(HCom,&message,14);
+}
+
+void CKobuki::setArcSpeed(int mmpersec, int radius)
+{
+    if (radius == 0) {
+        setTranslationSpeed(mmpersec);
+        return;
+    }
+
+    int speedvalue = mmpersec * ((radius + (radius>0? 230:-230) )/ 2 ) / radius;
+    unsigned char message[14] = { 0xaa,0x55,0x0A,0x0c,0x02,0xf0,0x00,0x01,0x04,speedvalue % 256,speedvalue >>8,radius % 256,radius >>8,  0x00 };
+    message[13] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^ message[7] ^ message[8] ^ message[9] ^ message[10] ^ message[11] ^ message[12];
+    uint32_t pocet;
+    pocet=write(HCom,&message,14);
+}
+
+void CKobuki::setSound(int noteinHz, int duration)
+{
+    int notevalue = floor((double)1.0 / ((double)noteinHz*0.00000275) + 0.5);
+    unsigned char message[9] = { 0xaa,0x55,0x05,0x03,0x03,notevalue%256,notevalue>>8,duration%256,0x00 };
+    message[8] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^ message[7];
+
+    uint32_t pocet;
+    pocet=write(HCom,&message,9);
+}
+
+
+
+
+void CKobuki::startCommunication(char * portname, bool CommandsEnabled,  void *userDataL)
+{
+    connect(portname);
+    enableCommands(CommandsEnabled);
+    userData = userDataL;
+
+    int pthread_result;
+    pthread_result = pthread_create(&threadHandle,NULL,KobukiProcess,(void *)this);
+}
+
+
+
+
+
+int CKobuki::measure()
+{
+    while (stopVlakno==0)
+    {
+        unsigned char *message = readKobukiMessage();
+        if (message == NULL)
+        {
+            printf("vratil null message\n");
+            continue;
+        }
+        int ok=parseKobukiMessage(data,message);
+
+        //maximalne moze trvat callback funkcia 20 ms, ak by trvala viac, nestihame citat
+        if (ok == 0)
+        {
+            loop(userData, data);
+        }
+        free(message);
+    }
+    return 0;
+}
+
+int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char * data)
+{
+    int rtrnvalue = checkChecksum(data);
+    //ak je zly checksum,tak kaslat na to
+    if (rtrnvalue != 0)
+        return -2;
+
+    int checkedValue = 1;
+    //kym neprejdeme celu dlzku
+    while (checkedValue < data[0])
+    {
+        //basic data subload
+        if (data[checkedValue] == 0x01)
+        {
+            checkedValue++;
+            if (data[checkedValue ] != 0x0F)
+                return -1;
+            checkedValue++;
+            output.timestamp = data[checkedValue + 1] * 256 + data[checkedValue];
+            checkedValue += 2;
+            output.BumperCenter = data[checkedValue ] && 0x02;
+            output.BumperLeft = data[checkedValue] && 0x04;
+            output.BumperRight = data[checkedValue] && 0x01;
+            checkedValue++;
+            output.WheelDropLeft= data[checkedValue] && 0x02;
+            output.WheelDropRight = data[checkedValue] && 0x01;
+            checkedValue++;
+            output.CliffCenter = data[checkedValue] && 0x02;
+            output.CliffLeft = data[checkedValue] && 0x04;
+            output.CliffRight = data[checkedValue] && 0x01;
+            checkedValue++;
+            output.EncoderLeft = data[checkedValue + 1] * 256 + data[checkedValue];
+            checkedValue += 2;
+            output.EncoderRight = data[checkedValue + 1] * 256 + data[checkedValue];
+            checkedValue += 2;
+            output.PWMleft = data[checkedValue] ;
+            checkedValue++;
+            output.PWMright = data[checkedValue] ;
+            checkedValue++;
+            output.ButtonPress = data[checkedValue];
+            checkedValue++;
+            output.Charger = data[checkedValue];
+            checkedValue++;
+            output.Battery = data[checkedValue];
+            checkedValue++;
+            output.overCurrent = data[checkedValue];
+            checkedValue++;
+        }
+        else if (data[checkedValue] == 0x03)
+        {
+            checkedValue++;
+            if (data[checkedValue] != 0x03)
+                return -3;
+            checkedValue++;
+            output.IRSensorRight = data[checkedValue];
+            checkedValue++;
+            output.IRSensorCenter = data[checkedValue];
+            checkedValue++;
+            output.IRSensorLeft = data[checkedValue];
+            checkedValue++;
+        }
+        else if (data[checkedValue] == 0x04)
+        {
+            checkedValue++;
+            if (data[checkedValue] != 0x07)
+                return -4;
+            checkedValue++;
+            output.GyroAngle = data[checkedValue + 1] * 256 + data[checkedValue];
+            checkedValue += 2;
+            output.GyroAngleRate = data[checkedValue + 1] * 256 + data[checkedValue];
+            checkedValue += 5;//3 unsued
+        }
+        else if (data[checkedValue] == 0x05)
+        {
+            checkedValue++;
+            if (data[checkedValue] != 0x06)
+                return -5;
+            checkedValue++;
+            output.CliffSensorRight = data[checkedValue + 1] * 256 + data[checkedValue];
+            checkedValue += 2;
+            output.CliffSensorCenter = data[checkedValue + 1] * 256 + data[checkedValue];
+            checkedValue += 2;
+            output.CliffSensorLeft = data[checkedValue + 1] * 256 + data[checkedValue];
+            checkedValue += 2;
+        }
+        else if (data[checkedValue] == 0x06)
+        {
+            checkedValue++;
+            if (data[checkedValue] != 0x02)
+                return -6;
+            checkedValue++;
+            output.wheelCurrentLeft =  data[checkedValue];
+            checkedValue ++;
+            output.wheelCurrentRight =data[checkedValue];
+            checkedValue ++;
+
+        }
+        else if (data[checkedValue] == 0x0A)
+        {
+            checkedValue++;
+            if (data[checkedValue] != 0x04)
+                return -7;
+            checkedValue++;
+            output.extraInfo.HardwareVersionPatch = data[checkedValue];
+            checkedValue++;
+            output.extraInfo.HardwareVersionMinor = data[checkedValue];
+            checkedValue++;
+            output.extraInfo.HardwareVersionMajor = data[checkedValue];
+            checkedValue += 2;
+
+        }
+        else if (data[checkedValue] == 0x0B)
+        {
+            checkedValue++;
+            if (data[checkedValue] != 0x04)
+                return -8;
+            checkedValue++;
+            output.extraInfo.FirmwareVersionPatch = data[checkedValue];
+            checkedValue++;
+            output.extraInfo.FirmwareVersionMinor = data[checkedValue];
+            checkedValue++;
+            output.extraInfo.FirmwareVersionMajor = data[checkedValue];
+            checkedValue += 2;
+
+        }
+        else if (data[checkedValue] == 0x0D)
+        {
+            checkedValue++;
+            if (data[checkedValue]%2 !=0)
+                return -9;
+            checkedValue++;
+            output.frameId = data[checkedValue];
+            checkedValue++;
+            int howmanyFrames = data[checkedValue]/3;
+            checkedValue++;
+            output.gyroData.reserve(howmanyFrames);
+            output.gyroData.clear();
+            for (int hk = 0; hk < howmanyFrames; hk++)
+            {
+                TRawGyroData temp;
+                temp.x = data[checkedValue + 1] * 256 + data[checkedValue];
+                checkedValue += 2;
+                temp.y = data[checkedValue + 1] * 256 + data[checkedValue];
+                checkedValue += 2;
+                temp.z = data[checkedValue + 1] * 256 + data[checkedValue];
+                checkedValue += 2;
+                output.gyroData.push_back(temp);
+            }
+        }
+        else if (data[checkedValue] == 0x10)
+        {
+            checkedValue++;
+            if (data[checkedValue] != 0x10)
+                return -10;
+            checkedValue++;
+            output.digitalInput = data[checkedValue + 1] * 256 + data[checkedValue];
+            checkedValue += 2;
+            output.analogInputCh0 = data[checkedValue + 1] * 256 + data[checkedValue];
+            checkedValue += 2;
+            output.analogInputCh1 = data[checkedValue + 1] * 256 + data[checkedValue];
+            checkedValue += 2;
+            output.analogInputCh2 = data[checkedValue + 1] * 256 + data[checkedValue];
+            checkedValue += 2;
+            output.analogInputCh3 = data[checkedValue + 1] * 256 + data[checkedValue];
+            checkedValue += 8;//2+6
+
+
+        }
+        else if (data[checkedValue] == 0x13)
+        {
+            checkedValue++;
+            if (data[checkedValue] != 0x0C)
+                return -11;
+            checkedValue++;
+            output.extraInfo.UDID0 = data[checkedValue + 3] * 256*256*256+ data[checkedValue + 2] * 256*256+ data[checkedValue + 1] * 256 + data[checkedValue];
+            checkedValue += 4;
+            output.extraInfo.UDID1 = data[checkedValue + 3] * 256 * 256 * 256 + data[checkedValue + 2] * 256 * 256 +data[checkedValue + 1] * 256 + data[checkedValue];
+            checkedValue += 4;
+            output.extraInfo.UDID2 = data[checkedValue + 3] * 256 * 256 * 256 + data[checkedValue + 2] * 256 * 256 +data[checkedValue + 1] * 256 + data[checkedValue];
+            checkedValue += 4;
+        }
+        else
+        {
+            checkedValue++;
+            checkedValue += data[checkedValue] + 1;
+        }
+    }
+    return 0;
+}
+
+
+
+
+
+
+
+long double CKobuki::gyroToRad(signed short GyroAngle) {
+
+    long double ret;
+    if (GyroAngle < 0) {
+        ret = GyroAngle + 36000;
+    }
+    else {
+        ret = GyroAngle;
+    }
+    return (long double) ret*PI/18000.0;
+}
+
+
+
+
+
+long CKobuki::loop(void *user_data, TKobukiData &Kobuki_data) {
+    if (iterationCount == 0) {
+        prevLeftEncoder = Kobuki_data.EncoderLeft;
+        prevRightEncoder = Kobuki_data.EncoderRight;
+        prevTimestamp = Kobuki_data.timestamp;
+        prevGyroTheta = gyroToRad(Kobuki_data.GyroAngle);
+        iterationCount++;
+    }
+
+    int dLeft;
+    if (abs(Kobuki_data.EncoderLeft - prevLeftEncoder) > 32000) {
+        dLeft = Kobuki_data.EncoderLeft - prevLeftEncoder + (Kobuki_data.EncoderLeft > prevLeftEncoder ? -65536 : +65536);
+    }
+    else {
+        dLeft = Kobuki_data.EncoderLeft - prevLeftEncoder;
+    }
+
+    int dRight;
+    if (abs(Kobuki_data.EncoderRight - prevRightEncoder) > 32000) {
+        dRight = Kobuki_data.EncoderRight - prevRightEncoder + (Kobuki_data.EncoderRight > prevRightEncoder ? -65536 : +65536);
+    }
+    else {
+        dRight = Kobuki_data.EncoderRight - prevRightEncoder;
+    }
+
+    long double dGyroTheta = prevGyroTheta - gyroToRad(Kobuki_data.GyroAngle);
+
+    if (dGyroTheta > PI) {
+        dGyroTheta -= 2*PI;
+    }
+    if (dGyroTheta < -1*PI) {
+        dGyroTheta += 2*PI;
+    }
+
+    gyroTheta += dGyroTheta;
+
+    uint16_t dTimestamp = Kobuki_data.timestamp - prevTimestamp;
+
+    long double mLeft = dLeft*tickToMeter;
+    long double mRight = dRight*tickToMeter;
+
+    if (mLeft == mRight) {
+        x = x + mRight;
+    } else {
+        x = x + (b*(mRight+mLeft))/(2*(mRight-mLeft))*(sin((mRight-mLeft)/b + theta) - sin(theta));
+        y = y + (b*(mRight+mLeft))/(2*(mRight-mLeft))*(cos((mRight-mLeft)/b + theta) - cos(theta));
+        theta = (mRight-mLeft)/b + theta;
+    }
+
+    displacement = (mRight + mLeft)/2;
+    integratedGyroTheta = integratedGyroTheta + dGyroTheta;
+    gx = gx + displacement * cos(integratedGyroTheta + dGyroTheta / 2);
+
+    gy = gy + displacement * sin(integratedGyroTheta + dGyroTheta / 2);
+
+
+
+
+
+
+    totalLeft +=dLeft;
+    totalRight +=dRight;
+
+    // ak je suma novej a predchadzajucej vacsia ako 65536 tak to pretieklo?
+    directionL = (prevLeftEncoder < Kobuki_data.EncoderLeft ? 1 : -1);
+    directionR = (prevRightEncoder < Kobuki_data.EncoderRight ? 1 : -1);
+    dTimestamp = (Kobuki_data.timestamp < prevTimestamp ? prevTimestamp - Kobuki_data.timestamp + 65536 : dTimestamp);
+
+
+    prevLeftEncoder = Kobuki_data.EncoderLeft;
+    prevRightEncoder = Kobuki_data.EncoderRight;
+    prevTimestamp = Kobuki_data.timestamp;
+    prevGyroTheta = gyroToRad(Kobuki_data.GyroAngle);
+
+
+//    std::cout << "X: " << x
+//              << " Y: " << y
+//              << " Theta: " << theta
+//              << "Gyro theta:" << gyroTheta
+//              << std::endl;
+
+
+    static long counter = 0;
+
+    vectorX.push_back(gx);
+    vectorY.push_back(gy);
+    vectorGyroTheta.push_back(gyroTheta);
+
+    if (counter % 100 == 0) {
+        p.plot_data(vectorY, vectorX);
+    }
+    counter++;
+
+    return 0;
+}
+
+
+
+
+// povie kobukimu ze ma ist niekolko metrov dopredu alebo dozadu, rozhoduje znamienko
+// funkcia kompenzuje chodenie rovno pomocou regulatora, interne vyuziva setArcSpeed a
+// ako spatnu vazbu pouziva data z enkoderov
+void CKobuki::goStraight(long double distance){
+    long double u_translation = 0; // riadena velicina, rychlost robota pri pohybe
+    long double w_translation = distance; // pozadovana hodnota
+
+    // parametre regulatora
+    long double Kp_translation = 4500;
+    long double e_translation = 0;
+    int upper_thresh_translation = 600;
+    int lower_thresh_translation = 40;
+    int translation_start_gain = 20;
+
+    long double u_rotation = 0; // riadena velicina
+    long double w_rotation = 0;
+    long double Kp_rotation = 57;
+    long double e_rotation = 0;
+
+    x = 0;
+    y = 0;
+    theta = 0;
+
+    long i = 5;
+
+    while (fabs(x - w_translation) > 0.005 && x<w_translation) {
+        e_translation = w_translation - x;
+        u_translation = Kp_translation * e_translation;
+
+        e_rotation = w_rotation - theta;
+        if (!e_rotation == 0) u_rotation = Kp_rotation / e_rotation;
+
+
+        // limit translation speed
+        if (u_translation > upper_thresh_translation)
+            u_translation = upper_thresh_translation;
+        if (u_translation < lower_thresh_translation)
+            u_translation = lower_thresh_translation;
+
+        // rewrite starting speed with line
+        if (i < u_translation) {
+            u_translation = i;
+        }
+
+        if (fabs(u_rotation) > 32767) {
+            u_rotation = -32767;
+        }
+
+        if (u_rotation == 0) {
+            u_rotation = -32767;
+        }
+
+        this->setArcSpeed(u_translation, u_rotation);
+
+        usleep(25*1000);
+        // increment starting speed
+        i = i + translation_start_gain;
+    }
+    this->setTranslationSpeed(0);
+}
+
+
+
+/// metoda vykona rotaciu, rotuje sa pomocou regulatora, ako spatna vazba sluzi gyroskop,
+/// kedze je radovo presnejsi ako enkodery
+void CKobuki::doRotation(long double th) {
+    long double u = 0; // riadena velicina, uhlova rychlost robota pri pohybe
+    long double w = th; // pozadovana hodnota v radianoch
+    long double Kp = PI;
+    long double e = 0;
+    int thresh = PI/2;
+
+    theta = 0;
+    x = 0;
+    y = 0;
+    gyroTheta = 0;
+
+    long double i = 0;
+
+    if (w > 0) {
+        while (gyroTheta < w) {
+            e = w - gyroTheta;
+            u = Kp*e;
+
+            if (u > thresh) u = thresh;
+            if (u < 0.4) u = 0.4;
+
+            if (i < u) {
+                u = i;
+            }
+
+            std::cout << "Angle: " << gyroTheta << " required:" << w << std::endl;
+            this->setRotationSpeed(-1*u);
+            usleep(25*1000);
+            i = i + 0.1;
+        }
+    }
+    else  {
+        while (gyroTheta > w) {
+            e = w - gyroTheta;
+            u = Kp*e*-1;
+
+            if (u > thresh) u = thresh;
+            if (u < 0.4) u = 0.4;
+
+            if (i < u) {
+                u = i;
+            }
+
+            std::cout << "Angle: " << gyroTheta << " required:" << w << std::endl;
+            this->setRotationSpeed(u);
+            usleep(25*1000);
+            i = i + 0.1;
+        }
+    }
+
+    std::cout << "stop the fuck!" << std::endl;
+    // usleep(25*1000);
+    this->setRotationSpeed(0);
+    usleep(25*1000);
+}
+
+
+// kombinuje navadzanie na suradnicu a rotaciu o uhol, realizuje presun na zvolenu suradnicu
+// v suradnicovom systeme robota
+void CKobuki::goToXy(long double xx, long double yy) {
+    long double th;
+
+    yy = yy*-1;
+
+    th = atan2(yy,xx);
+    doRotation(th);
+
+    long double s = sqrt(pow(xx,2)+pow(yy,2));
+
+    // resetnem suradnicovu sustavu robota
+    x = 0;
+    y = 0;
+    iterationCount = 0;
+    theta = 0;
+
+    //std::cout << "mam prejst: " << s << "[m]" << std::endl;
+
+    goStraight(s);
+
+    usleep(25*1000);
+    return;
+}
+
--- a/src/C++/Driver/src/CKobuki.h
+++ b/src/C++/Driver/src/CKobuki.h
@@ -0,0 +1,224 @@
+//#pragma once
+////*************************************************************************************
+////*************************************************************************************
+//// autor Martin Dekan  a Peter Beno mail: dekdekan@gmail.com, peter.beno@stuba.sk
+////-------------------------------------------------------------------------------------
+//// co to je:
+//// trieda na pracu s robotom kobuki. mala by mat implementovane citanie dat
+//// a posielanie prikazov...
+//// neobsahuje ziadnu logiku co s datami robit, to je na userovi aby spravil v callback funkcii
+//  jedna sa vlastne len o implementaciu komunikacie s hardwareom, driver
+////*************************************************************************************
+////*************************************************************************************
+#ifndef KOBUKI_CLASS_123456789
+#define KOBUKI_CLASS_123456789
+#define PI          3.141592653589793238462643383279502884L /* pi */
+#define MS_INSTRUCTION_DELAY 25
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <vector>
+#include "pthread.h"
+#include "unistd.h"
+#include "fcntl.h"
+#include "string.h"
+#include <math.h>
+#include <stdint.h>
+#include <iostream>
+#include <fstream>
+#include <cmath>
+#include <iomanip>
+#include <chrono>
+#include <sstream>
+
+#include "graph.h"
+
+using namespace std;
+
+typedef struct
+{
+	
+	unsigned short x;
+	unsigned short y;
+	unsigned short z;
+
+}TRawGyroData;
+typedef struct
+{
+	//Hardware Version
+	unsigned char HardwareVersionMajor;
+	unsigned char HardwareVersionMinor;
+	unsigned char HardwareVersionPatch;
+	//Firmware Version
+	unsigned char FirmwareVersionMajor;
+	unsigned char FirmwareVersionMinor;
+	unsigned char FirmwareVersionPatch;
+
+	//Unique Device IDentifier(UDID)
+	unsigned int UDID0;
+	unsigned int UDID1;
+	unsigned int UDID2;
+	//Controller Info
+	unsigned char PIDtype;
+	unsigned int PIDgainP;
+	unsigned int PIDgainI;
+	unsigned int PIDgainD;
+}TExtraRequestData;
+
+typedef struct
+{
+	//---zakladny balik
+	unsigned short timestamp;
+	//narazniky
+	bool BumperLeft;
+	bool BumperCenter;
+	bool BumperRight;
+	//cliff
+	bool CliffLeft;
+	bool CliffCenter;
+	bool CliffRight;
+	// padnutie kolies
+	bool WheelDropLeft;  
+	bool WheelDropRight; 
+	//tocenie kolies
+	unsigned short EncoderRight;
+	unsigned short EncoderLeft;
+	unsigned char PWMright;
+	unsigned char PWMleft;
+	//gombiky
+	unsigned char ButtonPress;// 0 nie, 1 2 4 pre button 0 1 2 (7 je ze vsetky tri)
+	//napajanie
+	unsigned char Charger;
+	unsigned char Battery;
+	unsigned char overCurrent;
+	//---docking ir
+	unsigned char IRSensorRight;
+	unsigned char IRSensorCenter;
+	unsigned char IRSensorLeft;
+	//---Inertial Sensor Data
+	signed short GyroAngle;
+	unsigned short GyroAngleRate;
+	//---Cliff Sensor Data
+	unsigned short CliffSensorRight;
+	unsigned short CliffSensorCenter;
+	unsigned short CliffSensorLeft;
+	//---Current
+	unsigned char wheelCurrentLeft;
+	unsigned char wheelCurrentRight;
+	//---Raw Data Of 3D Gyro
+	unsigned char frameId;
+	std::vector<TRawGyroData> gyroData;
+	//---General Purpose Input
+	unsigned short digitalInput;
+	unsigned short analogInputCh0;
+	unsigned short analogInputCh1;
+	unsigned short analogInputCh2;
+	unsigned short analogInputCh3;
+	//---struktura s datami ktore sa nam tam objavia iba na poziadanie
+	TExtraRequestData extraInfo;
+}TKobukiData;
+
+
+typedef long(*src_callback_kobuki_data) (void *user_data, TKobukiData &Kobuki_data);
+
+class CKobuki
+{
+public:
+	CKobuki() { 
+		stopVlakno = 0; 
+		std::cout << "kobuki instantiated" << std::endl;
+  		odometry_log.open("odometry.txt");
+	};
+	 virtual ~CKobuki() { 
+		stopVlakno = 1; 
+	 	close(HCom);
+		pthread_cancel(threadHandle); 
+		odometry_log.close();
+	};
+	
+	void enableCommands(bool commands) {
+		enabledCommands = commands;
+	};
+
+
+	long loop(void *user_data, TKobukiData &Kobuki_data);
+
+	void startCommunication(char *portname,bool CommandsEnabled,void *userDataL);
+	int measure(); //vlaknova funkcia, ma v sebe nekonecne vlakno a vycitava udaje
+	void setLed(int led1 = 0, int led2 = 0); //led1 zelena/cervena 2/1, //led2 zelena/cervena 2/1
+	void setTranslationSpeed(int mmpersec);
+	void setRotationSpeed(double radpersec);
+	void setArcSpeed(int mmpersec,int radius);
+	void setSound(int noteinHz, int duration);
+	void setPower(int value);
+
+	// control functions 
+	void goStraight(long double distance);
+	void doRotation(long double th);
+	void goToXy(long double xx, long double yy);
+	std::ofstream odometry_log;
+
+
+
+
+private:
+	int HCom;
+	pthread_t threadHandle; // handle na vlakno
+	int threadID;  // id vlakna
+	int stopVlakno;
+	TKobukiData data;
+	src_callback_kobuki_data callbackFunction;
+	void *userData;
+	bool enabledCommands;
+	int parseKobukiMessage(TKobukiData &output, unsigned char *data );
+	int connect(char *portname);
+	unsigned char *readKobukiMessage();
+	int checkChecksum(unsigned char *data);
+	
+	//--spustenie merania v novom vlakne (vycitavanie bezi v novom vlakne. treba ho stopnut ak chceme poslat request)
+	static void * KobukiProcess(void *param)
+	{
+		//std::cout << "Nase vlakno Kobuki process nastartovalo" << std::endl;
+		CKobuki *hoku = (CKobuki*)param;
+		int vystup = hoku->measure();
+		
+		return param;
+	}
+
+
+	// internal variables for robot control
+	int prevLeftEncoder, prevRightEncoder; // [ticks]
+	uint16_t prevTimestamp;// [ms]
+	long totalLeft, totalRight = 0;
+	int directionL = 0; // 1 = forward, 0 = undefined, -1 = backwards
+	int directionR = 0;
+	int iterationCount = 0;
+	long double tickToMeter = 0.000085292090497737556558; // [m/tick]
+//
+	long double x = 0; // [m]
+	long double y = 0;
+//
+	long double theta = 0; // [rad]
+	long double b = 0.23; // wheelbase distance in meters, from kobuki manual https://yujinrobot.github.io/kobuki/doxygen/enAppendixProtocolSpecification.html
+//
+	long double prevGyroTheta = 0;
+	long double gyroTheta = 0; // [rad]
+
+	// utilities
+	long double gyroToRad(signed short GyroAngle);
+
+    plot p;
+    std::vector<float> vectorX;
+    std::vector<float> vectorY;
+    std::vector<float> vectorGyroTheta;
+
+
+    double displacement = 0;
+    double integratedGyroTheta = 0;
+    double gx = 0;
+    double gy = 0;
+
+
+};
+
+#endif
--- a/src/C++/Driver/src/graph.h
+++ b/src/C++/Driver/src/graph.h
@@ -0,0 +1,71 @@
+#ifndef GRAPH1010
+#define GRAPH1010
+#include <stdio.h>
+#include <stdlib.h>
+#include <vector>
+
+using namespace std;
+#define GRAPH_ENABLED true
+
+class plot {
+public:
+ FILE *gp;
+ bool enabled,persist;
+ plot(bool _persist=false,bool _enabled=GRAPH_ENABLED) {
+ enabled=_enabled;
+ persist=_persist;
+  if (enabled) {
+	if(persist)
+		  gp=popen("gnuplot -persist","w");
+	else
+		  gp=popen("gnuplot","w");
+  }
+ }
+
+ void plot_data(vector<float> x,const char* style="points",const char* title="Data") {
+ if(!enabled)
+	return;
+  fprintf(gp,"set title '%s' \n",title);
+  fprintf(gp,"plot '-' w %s \n",style);
+  for(int k=0;k<x.size();k++) {
+   fprintf(gp,"%f\n",x[k]);
+  }
+  fprintf(gp,"e\n");
+  fflush(gp);
+ }
+
+ void plot_data(vector<float> x,vector<float> y,const char* style="points",const char* title="Data") {
+ if(!enabled)
+	return;
+  fprintf(gp,"set title '%s' \n",title);
+  fprintf(gp,"plot '-' w %s \n",style);
+  for(int k=0;k<x.size();k++) {
+   fprintf(gp,"%f %f \n",x[k],y[k]);
+  }
+  fprintf(gp,"e\n");
+  fflush(gp);
+ }
+
+ ~plot() {
+  if(enabled)
+  pclose(gp);
+ }
+ 
+};
+
+/*
+int main(int argc,char **argv) {
+ plot p;
+ for(int a=0;a<100;a++) {
+ vector<float> x,y;
+ for(int k=a;k<a+200;k++) {
+   x.push_back(k);
+   y.push_back(k*k);
+ }
+  p.plot_data(x,y);
+ }
+ return 0;
+}
+*/
+
+#endif
--- a/src/C++/Driver/src/main.cpp
+++ b/src/C++/Driver/src/main.cpp
@@ -0,0 +1,22 @@
+#include "CKobuki.h"
+#include <iostream>
+#include <cmath>
+
+#include "graph.h"
+
+using namespace std;
+
+int main() {
+	unsigned char * null_ptr(0);
+	CKobuki robot;
+
+	robot.startCommunication("/dev/ttyUSB0", true, null_ptr);
+	usleep(1*1000*1000);
+
+	robot.goStraight(10);
+
+
+
+
+	usleep(30*1000*1000);
+}