diff --git a/src/C++/Driver/.gitignore b/src/C++/Driver/.gitignore new file mode 100644 index 0000000..1d23b8b --- /dev/null +++ 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/* + diff --git a/src/C++/Driver/CMakeLists.txt b/src/C++/Driver/CMakeLists.txt new file mode 100644 index 0000000..9c66fba --- /dev/null +++ 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 ) \ No newline at end of file diff --git a/src/C++/Driver/LICENSE b/src/C++/Driver/LICENSE new file mode 100644 index 0000000..4d91c5a --- /dev/null +++ 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. diff --git a/src/C++/Driver/Makefile b/src/C++/Driver/Makefile new file mode 100644 index 0000000..ea53047 --- /dev/null +++ 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 + diff --git a/src/C++/Driver/cmake_install.cmake b/src/C++/Driver/cmake_install.cmake new file mode 100644 index 0000000..45179d9 --- /dev/null +++ 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() diff --git a/src/C++/Driver/src/CKobuki.cpp b/src/C++/Driver/src/CKobuki.cpp new file mode 100755 index 0000000..8010067 --- /dev/null +++ b/src/C++/Driver/src/CKobuki.cpp @@ -0,0 +1,740 @@ +#include "CKobuki.h" +#include "termios.h" +#include "errno.h" +#include +#include + +plot p; +static std::vector vectorX; +static std::vector vectorY; +static std::vector 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>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 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; +} + diff --git a/src/C++/Driver/src/CKobuki.h b/src/C++/Driver/src/CKobuki.h new file mode 100755 index 0000000..05a19fd --- /dev/null +++ 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 +#include +#include +#include "pthread.h" +#include "unistd.h" +#include "fcntl.h" +#include "string.h" +#include +#include +#include +#include +#include +#include +#include +#include + +#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 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 vectorX; + std::vector vectorY; + std::vector vectorGyroTheta; + + + double displacement = 0; + double integratedGyroTheta = 0; + double gx = 0; + double gy = 0; + + +}; + +#endif diff --git a/src/C++/Driver/src/graph.h b/src/C++/Driver/src/graph.h new file mode 100644 index 0000000..89c963c --- /dev/null +++ b/src/C++/Driver/src/graph.h @@ -0,0 +1,71 @@ +#ifndef GRAPH1010 +#define GRAPH1010 +#include +#include +#include + +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 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,vector 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,y; + for(int k=a;k +#include + +#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); +} diff --git a/src/C++/main.cpp b/src/C++/main.cpp index 5065b0f..b78a180 100644 --- a/src/C++/main.cpp +++ b/src/C++/main.cpp @@ -1,9 +1,12 @@ //before running set right baudrate //sudo stty -F /dev/ttyUSB0 speed 115200 cs8 -cstopb -parenb +//TODO: Fix the first output of the buffer #include #include #include #include +#include + int main() { char byte; @@ -21,9 +24,16 @@ int main() { if (foundAA) { if (ubyte == 0b01010101) { // 0x55 std::cout << "Found 0xAA followed by 0x55" << std::endl; - std::cout << buffer.size() << std::endl; //display size of buffer + // std::cout << "Buffer size" << buffer.size() << std::endl << std::endl; + // Print the buffer in hex values + for (int i = 0; i < 4; i++) { + std::cout << "0x" << std::hex << std::setw(2) << std::setfill('0') << static_cast(static_cast(buffer[i])) << " "; + } + //reset buffer after new 0XAA and 0x55 is found buffer.clear(); foundAA = false; // Reset the state + if (ubyte == 0b10101010) + foundAA = true; } else { foundAA = false; // Reset the state if the next byte is not 0x55 } diff --git a/src/C++/main.o b/src/C++/main.o index fc78527..d2cd5c5 100644 Binary files a/src/C++/main.o and b/src/C++/main.o differ diff --git a/src/C++/progr b/src/C++/progr index 284b176..4984e9b 100755 Binary files a/src/C++/progr and b/src/C++/progr differ