Smikkelbakje/J2S1-Kobuki
1
0
Fork 0
mirror of https://gitlab.fdmci.hva.nl/technische-informatica-sm3/ti-projectten/rooziinuubii79.git synced 2025-08-03 20:04:58 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

Merge branch 'parser-cleanup' into 'main'

Browse Source 
parser rewrite and put into its own class

See merge request technische-informatica-sm3/ti-projectten/rooziinuubii79!1
This commit is contained in:
Sam Hos
2024-10-07 21:34:47 +02:00
parent 83b3e9f67f 9370be2794
commit 4279a11160
9 changed files with 620 additions and 575 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
.gitignore vendored
View File

@@ -12,3 +12,7 @@ src/C++/Driver/vgcore.42611
src/Socket/a.out src/Socket/a.out
src/C++/Driver/cmake_install.cmake src/C++/Driver/cmake_install.cmake
src/C++/Socket/a.out src/C++/Socket/a.out
src/C++/Driver/Makefile
src/C++/Driver/vgcore*
src/C++/Driver/cmake_install.cmake
src/C++/Driver/Makefile

7
src/C++/Driver/CMakeLists.txt
View File

@@ -1,13 +1,12 @@
cmake_minimum_required(VERSION 3.9) cmake_minimum_required(VERSION 3.9)
project(kobuki_control) project(kobuki_control)
#set(CMAKE_CXX_STANDARD 11)
find_package( OpenCV REQUIRED )
set(SOURCE_FILES set(SOURCE_FILES
src/KobukiParser.cpp
src/KobukiParser.h
src/CKobuki.cpp src/CKobuki.cpp
src/CKobuki.h src/CKobuki.h
src/test.cpp) src/main.cpp)
add_executable(kobuki_control ${SOURCE_FILES}) add_executable(kobuki_control ${SOURCE_FILES})
#target_link_libraries(kobuki_control ) #target_link_libraries(kobuki_control )

208
src/C++/Driver/Makefile
View File

@@ -1,208 +0,0 @@
# 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

62
src/C++/Driver/cmake_install.cmake
View File

@@ -1,62 +0,0 @@
# 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()

414
src/C++/Driver/src/CKobuki.cpp
View File

@@ -4,6 +4,7 @@
#include <cstddef> #include <cstddef>
#include <iostream> #include <iostream>
// plot p; // plot p;
static std::vector<float> vectorX; static std::vector<float> vectorX;
static std::vector<float> vectorY; static std::vector<float> vectorY;
@@ -163,16 +164,6 @@ unsigned char *CKobuki::readKobukiMessage()
return null_buffer; 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) void CKobuki::setLed(int led1, int led2)
{ {
unsigned char message[8] = {0xaa, 0x55, 0x04, 0x0c, 0x02, 0x00, (unsigned char)((led1 + led2 * 4) % 256), 0x00}; unsigned char message[8] = {0xaa, 0x55, 0x04, 0x0c, 0x02, 0x00, (unsigned char)((led1 + led2 * 4) % 256), 0x00};
@@ -275,16 +266,17 @@ void CKobuki::setSound(int noteinHz, int duration)
pocet = write(HCom, &message, 9); pocet = write(HCom, &message, 9);
} }
void CKobuki::startCommunication(char *portname, bool CommandsEnabled, void CKobuki::startCommunication(char *portname, bool CommandsEnabled, void *userDataL)
void *userDataL)
{ {
connect(portname); connect(portname);
enableCommands(CommandsEnabled); enableCommands(CommandsEnabled);
userData = userDataL; userData = userDataL;
int pthread_result; int pthread_result;
pthread_result = pthread_result = pthread_create(&threadHandle, NULL, KobukiProcess, (void *)this);
pthread_create(&threadHandle, NULL, KobukiProcess, (void *)this); if (pthread_result != 0) {
std::cerr << "Error creating thread: " << pthread_result << std::endl;
}
} }
int CKobuki::measure() int CKobuki::measure()
@@ -297,215 +289,215 @@ int CKobuki::measure()
// printf("vratil null message\n"); // printf("vratil null message\n");
continue; continue;
} }
int ok = parseKobukiMessage(data, message); int ok = parser.parseKobukiMessage(parser.data, message);
// maximalne moze trvat callback funkcia 20 ms, ak by trvala viac, nestihame // maximalne moze trvat callback funkcia 20 ms, ak by trvala viac, nestihame
// citat // citat
if (ok == 0) if (ok == 0)
{ {
loop(userData, data); loop(userData, parser.data);
} }
free(message); free(message);
} }
return 0; return 0;
} }
int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char *data) // int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char *data)
{ // {
int rtrnvalue = checkChecksum(data); // int rtrnvalue = checkChecksum(data);
// ak je zly checksum,tak kaslat na to // // ak je zly checksum,tak kaslat na to
if (rtrnvalue != 0) // if (rtrnvalue != 0)
return -2; // return -2;
int checkedValue = 1; // int checkedValue = 1;
// kym neprejdeme celu dlzku // // kym neprejdeme celu dlzku
while (checkedValue < data[0]) // while (checkedValue < data[0])
{ // {
// basic data subload // // basic data subload
if (data[checkedValue] == 0x01) // if (data[checkedValue] == 0x01)
{ // {
checkedValue++; // checkedValue++;
if (data[checkedValue] != 0x0F) // if (data[checkedValue] != 0x0F)
return -1; // return -1;
checkedValue++; // checkedValue++;
output.timestamp = data[checkedValue + 1] * 256 + data[checkedValue]; // output.timestamp = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; // checkedValue += 2;
output.BumperCenter = data[checkedValue] && 0x02; // output.BumperCenter = data[checkedValue] && 0x02;
output.BumperLeft = data[checkedValue] && 0x04; // output.BumperLeft = data[checkedValue] && 0x04;
output.BumperRight = data[checkedValue] && 0x01; // output.BumperRight = data[checkedValue] && 0x01;
checkedValue++; // checkedValue++;
output.WheelDropLeft = data[checkedValue] && 0x02; // output.WheelDropLeft = data[checkedValue] && 0x02;
output.WheelDropRight = data[checkedValue] && 0x01; // output.WheelDropRight = data[checkedValue] && 0x01;
checkedValue++; // checkedValue++;
output.CliffCenter = data[checkedValue] && 0x02; // output.CliffCenter = data[checkedValue] && 0x02;
output.CliffLeft = data[checkedValue] && 0x04; // output.CliffLeft = data[checkedValue] && 0x04;
output.CliffRight = data[checkedValue] && 0x01; // output.CliffRight = data[checkedValue] && 0x01;
checkedValue++; // checkedValue++;
output.EncoderLeft = data[checkedValue + 1] * 256 + data[checkedValue]; // output.EncoderLeft = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; // checkedValue += 2;
output.EncoderRight = data[checkedValue + 1] * 256 + data[checkedValue]; // output.EncoderRight = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; // checkedValue += 2;
output.PWMleft = data[checkedValue]; // output.PWMleft = data[checkedValue];
checkedValue++; // checkedValue++;
output.PWMright = data[checkedValue]; // output.PWMright = data[checkedValue];
checkedValue++; // checkedValue++;
output.ButtonPress = data[checkedValue]; // output.ButtonPress = data[checkedValue];
checkedValue++; // checkedValue++;
output.Charger = data[checkedValue]; // output.Charger = data[checkedValue];
checkedValue++; // checkedValue++;
output.Battery = data[checkedValue]; // output.Battery = data[checkedValue];
checkedValue++; // checkedValue++;
output.overCurrent = data[checkedValue]; // output.overCurrent = data[checkedValue];
checkedValue++; // checkedValue++;
} // }
else if (data[checkedValue] == 0x03) // else if (data[checkedValue] == 0x03)
{ // {
checkedValue++; // checkedValue++;
if (data[checkedValue] != 0x03) // if (data[checkedValue] != 0x03)
return -3; // return -3;
checkedValue++; // checkedValue++;
output.IRSensorRight = data[checkedValue]; // output.IRSensorRight = data[checkedValue];
checkedValue++; // checkedValue++;
output.IRSensorCenter = data[checkedValue]; // output.IRSensorCenter = data[checkedValue];
checkedValue++; // checkedValue++;
output.IRSensorLeft = data[checkedValue]; // output.IRSensorLeft = data[checkedValue];
checkedValue++; // checkedValue++;
} // }
else if (data[checkedValue] == 0x04) // else if (data[checkedValue] == 0x04)
{ // {
checkedValue++; // checkedValue++;
if (data[checkedValue] != 0x07) // if (data[checkedValue] != 0x07)
return -4; // return -4;
checkedValue++; // checkedValue++;
output.GyroAngle = data[checkedValue + 1] * 256 + data[checkedValue]; // output.GyroAngle = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; // checkedValue += 2;
output.GyroAngleRate = data[checkedValue + 1] * 256 + data[checkedValue]; // output.GyroAngleRate = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 5; // 3 unsued // checkedValue += 5; // 3 unsued
} // }
else if (data[checkedValue] == 0x05) // else if (data[checkedValue] == 0x05)
{ // {
checkedValue++; // checkedValue++;
if (data[checkedValue] != 0x06) // if (data[checkedValue] != 0x06)
return -5; // return -5;
checkedValue++; // checkedValue++;
output.CliffSensorRight = // output.CliffSensorRight =
data[checkedValue + 1] * 256 + data[checkedValue]; // data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; // checkedValue += 2;
output.CliffSensorCenter = // output.CliffSensorCenter =
data[checkedValue + 1] * 256 + data[checkedValue]; // data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; // checkedValue += 2;
output.CliffSensorLeft = // output.CliffSensorLeft =
data[checkedValue + 1] * 256 + data[checkedValue]; // data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; // checkedValue += 2;
} // }
else if (data[checkedValue] == 0x06) // else if (data[checkedValue] == 0x06)
{ // {
checkedValue++; // checkedValue++;
if (data[checkedValue] != 0x02) // if (data[checkedValue] != 0x02)
return -6; // return -6;
checkedValue++; // checkedValue++;
output.wheelCurrentLeft = data[checkedValue]; // output.wheelCurrentLeft = data[checkedValue];
checkedValue++; // checkedValue++;
output.wheelCurrentRight = data[checkedValue]; // output.wheelCurrentRight = data[checkedValue];
checkedValue++; // checkedValue++;
} // }
else if (data[checkedValue] == 0x0A) // else if (data[checkedValue] == 0x0A)
{ // {
checkedValue++; // checkedValue++;
if (data[checkedValue] != 0x04) // if (data[checkedValue] != 0x04)
return -7; // return -7;
checkedValue++; // checkedValue++;
output.extraInfo.HardwareVersionPatch = data[checkedValue]; // output.extraInfo.HardwareVersionPatch = data[checkedValue];
checkedValue++; // checkedValue++;
output.extraInfo.HardwareVersionMinor = data[checkedValue]; // output.extraInfo.HardwareVersionMinor = data[checkedValue];
checkedValue++; // checkedValue++;
output.extraInfo.HardwareVersionMajor = data[checkedValue]; // output.extraInfo.HardwareVersionMajor = data[checkedValue];
checkedValue += 2; // checkedValue += 2;
} // }
else if (data[checkedValue] == 0x0B) // else if (data[checkedValue] == 0x0B)
{ // {
checkedValue++; // checkedValue++;
if (data[checkedValue] != 0x04) // if (data[checkedValue] != 0x04)
return -8; // return -8;
checkedValue++; // checkedValue++;
output.extraInfo.FirmwareVersionPatch = data[checkedValue]; // output.extraInfo.FirmwareVersionPatch = data[checkedValue];
checkedValue++; // checkedValue++;
output.extraInfo.FirmwareVersionMinor = data[checkedValue]; // output.extraInfo.FirmwareVersionMinor = data[checkedValue];
checkedValue++; // checkedValue++;
output.extraInfo.FirmwareVersionMajor = data[checkedValue]; // output.extraInfo.FirmwareVersionMajor = data[checkedValue];
checkedValue += 2; // checkedValue += 2;
} // }
else if (data[checkedValue] == 0x0D) // else if (data[checkedValue] == 0x0D)
{ // {
checkedValue++; // checkedValue++;
if (data[checkedValue] % 2 != 0) // if (data[checkedValue] % 2 != 0)
return -9; // return -9;
checkedValue++; // checkedValue++;
output.frameId = data[checkedValue]; // output.frameId = data[checkedValue];
checkedValue++; // checkedValue++;
int howmanyFrames = data[checkedValue] / 3; // int howmanyFrames = data[checkedValue] / 3;
checkedValue++; // checkedValue++;
output.gyroData.reserve(howmanyFrames); // output.gyroData.reserve(howmanyFrames);
output.gyroData.clear(); // output.gyroData.clear();
for (int hk = 0; hk < howmanyFrames; hk++) // for (int hk = 0; hk < howmanyFrames; hk++)
{ // {
TRawGyroData temp; // TRawGyroData temp;
temp.x = data[checkedValue + 1] * 256 + data[checkedValue]; // temp.x = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; // checkedValue += 2;
temp.y = data[checkedValue + 1] * 256 + data[checkedValue]; // temp.y = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; // checkedValue += 2;
temp.z = data[checkedValue + 1] * 256 + data[checkedValue]; // temp.z = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; // checkedValue += 2;
output.gyroData.push_back(temp); // output.gyroData.push_back(temp);
} // }
} // }
else if (data[checkedValue] == 0x10) // else if (data[checkedValue] == 0x10)
{ // {
checkedValue++; // checkedValue++;
if (data[checkedValue] != 0x10) // if (data[checkedValue] != 0x10)
return -10; // return -10;
checkedValue++; // checkedValue++;
output.digitalInput = data[checkedValue + 1] * 256 + data[checkedValue]; // output.digitalInput = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; // checkedValue += 2;
output.analogInputCh0 = data[checkedValue + 1] * 256 + data[checkedValue]; // output.analogInputCh0 = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; // checkedValue += 2;
output.analogInputCh1 = data[checkedValue + 1] * 256 + data[checkedValue]; // output.analogInputCh1 = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; // checkedValue += 2;
output.analogInputCh2 = data[checkedValue + 1] * 256 + data[checkedValue]; // output.analogInputCh2 = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; // checkedValue += 2;
output.analogInputCh3 = data[checkedValue + 1] * 256 + data[checkedValue]; // output.analogInputCh3 = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 8; // 2+6 // checkedValue += 8; // 2+6
} // }
else if (data[checkedValue] == 0x13) // else if (data[checkedValue] == 0x13)
{ // {
checkedValue++; // checkedValue++;
if (data[checkedValue] != 0x0C) // if (data[checkedValue] != 0x0C)
return -11; // return -11;
checkedValue++; // checkedValue++;
output.extraInfo.UDID0 = data[checkedValue + 3] * 256 * 256 * 256 + // output.extraInfo.UDID0 = data[checkedValue + 3] * 256 * 256 * 256 +
data[checkedValue + 2] * 256 * 256 + // data[checkedValue + 2] * 256 * 256 +
data[checkedValue + 1] * 256 + // data[checkedValue + 1] * 256 +
data[checkedValue]; // data[checkedValue];
checkedValue += 4; // checkedValue += 4;
output.extraInfo.UDID1 = data[checkedValue + 3] * 256 * 256 * 256 + // output.extraInfo.UDID1 = data[checkedValue + 3] * 256 * 256 * 256 +
data[checkedValue + 2] * 256 * 256 + // data[checkedValue + 2] * 256 * 256 +
data[checkedValue + 1] * 256 + // data[checkedValue + 1] * 256 +
data[checkedValue]; // data[checkedValue];
checkedValue += 4; // checkedValue += 4;
output.extraInfo.UDID2 = data[checkedValue + 3] * 256 * 256 * 256 + // output.extraInfo.UDID2 = data[checkedValue + 3] * 256 * 256 * 256 +
data[checkedValue + 2] * 256 * 256 + // data[checkedValue + 2] * 256 * 256 +
data[checkedValue + 1] * 256 + // data[checkedValue + 1] * 256 +
data[checkedValue]; // data[checkedValue];
checkedValue += 4; // checkedValue += 4;
} // }
else // else
{ // {
checkedValue++; // checkedValue++;
checkedValue += data[checkedValue] + 1; // checkedValue += data[checkedValue] + 1;
} // }
} // }
return 0; // return 0;
} // }
long double CKobuki::gyroToRad(signed short GyroAngle) long double CKobuki::gyroToRad(signed short GyroAngle)
{ {

166
src/C++/Driver/src/CKobuki.h
View File

@@ -30,93 +30,94 @@
#include <iomanip> #include <iomanip>
#include <chrono> #include <chrono>
#include <sstream> #include <sstream>
#include "KobukiParser.h"
#include "graph.h" #include "graph.h"
using namespace std; using namespace std;
typedef struct
{
unsigned short x; // typedef struct
unsigned short y; // {
unsigned short z;
}TRawGyroData; // unsigned short x;
typedef struct // unsigned short y;
{ // unsigned short z;
//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) // }TRawGyroData;
unsigned int UDID0; // typedef struct
unsigned int UDID1; // {
unsigned int UDID2; // //Hardware Version
//Controller Info // unsigned char HardwareVersionMajor;
unsigned char PIDtype; // unsigned char HardwareVersionMinor;
unsigned int PIDgainP; // unsigned char HardwareVersionPatch;
unsigned int PIDgainI; // //Firmware Version
unsigned int PIDgainD; // unsigned char FirmwareVersionMajor;
}TExtraRequestData; // unsigned char FirmwareVersionMinor;
// unsigned char FirmwareVersionPatch;
typedef struct // //Unique Device IDentifier(UDID)
{ // unsigned int UDID0;
//---basic package // unsigned int UDID1;
unsigned short timestamp; // unsigned int UDID2;
//bumpers // //Controller Info
bool BumperLeft; // unsigned char PIDtype;
bool BumperCenter; // unsigned int PIDgainP;
bool BumperRight; // unsigned int PIDgainI;
//cliff // unsigned int PIDgainD;
bool CliffLeft; // }TExtraRequestData;
bool CliffCenter;
bool CliffRight; // typedef struct
// wheel drop // {
bool WheelDropLeft; // //---basic package
bool WheelDropRight; // unsigned short timestamp;
//wheel rotation // //bumpers
unsigned short EncoderRight; // bool BumperLeft;
unsigned short EncoderLeft; // bool BumperCenter;
unsigned char PWMright; // bool BumperRight;
unsigned char PWMleft; // //cliff
//buttons // bool CliffLeft;
unsigned char ButtonPress;// 0 no, 1 2 4 for button 0 1 2 (7 is all three) // bool CliffCenter;
//power // bool CliffRight;
unsigned char Charger; // // wheel drop
unsigned char Battery; // bool WheelDropLeft;
unsigned char overCurrent; // bool WheelDropRight;
//---docking ir // //wheel rotation
unsigned char IRSensorRight; // unsigned short EncoderRight;
unsigned char IRSensorCenter; // unsigned short EncoderLeft;
unsigned char IRSensorLeft; // unsigned char PWMright;
//---Inertial Sensor Data // unsigned char PWMleft;
signed short GyroAngle; // //buttons
unsigned short GyroAngleRate; // unsigned char ButtonPress;// 0 no, 1 2 4 for button 0 1 2 (7 is all three)
//---Cliff Sensor Data // //power
unsigned short CliffSensorRight; // unsigned char Charger;
unsigned short CliffSensorCenter; // unsigned char Battery;
unsigned short CliffSensorLeft; // unsigned char overCurrent;
//---Current // //---docking ir
unsigned char wheelCurrentLeft; // unsigned char IRSensorRight;
unsigned char wheelCurrentRight; // unsigned char IRSensorCenter;
//---Raw Data Of 3D Gyro // unsigned char IRSensorLeft;
unsigned char frameId; // //---Inertial Sensor Data
std::vector<TRawGyroData> gyroData; // signed short GyroAngle;
//---General Purpose Input // unsigned short GyroAngleRate;
unsigned short digitalInput; // //---Cliff Sensor Data
unsigned short analogInputCh0; // unsigned short CliffSensorRight;
unsigned short analogInputCh1; // unsigned short CliffSensorCenter;
unsigned short analogInputCh2; // unsigned short CliffSensorLeft;
unsigned short analogInputCh3; // //---Current
//---structure with data that appears only on request // unsigned char wheelCurrentLeft;
TExtraRequestData extraInfo; // unsigned char wheelCurrentRight;
}TKobukiData; // //---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;
// //---structure with data that appears only on request
// TExtraRequestData extraInfo;
// }TKobukiData;
typedef long(*src_callback_kobuki_data) (void *user_data, TKobukiData &Kobuki_data); typedef long(*src_callback_kobuki_data) (void *user_data, TKobukiData &Kobuki_data);
@@ -124,6 +125,7 @@ typedef long(*src_callback_kobuki_data) (void *user_data, TKobukiData &Kobuki_da
class CKobuki class CKobuki
{ {
public: public:
CKobuki() { CKobuki() {
stopVlakno = 0; stopVlakno = 0;
std::cout << "kobuki instantiated" << std::endl; std::cout << "kobuki instantiated" << std::endl;
@@ -158,12 +160,14 @@ public:
void doRotation(long double th); void doRotation(long double th);
void goToXy(long double xx, long double yy); void goToXy(long double xx, long double yy);
std::ofstream odometry_log; std::ofstream odometry_log;
TKobukiData data; KobukiParser parser;
private: private:
int HCom; int HCom;
pthread_t threadHandle; // handle to the thread pthread_t threadHandle; // handle to the thread
int threadID; // thread id int threadID; // thread id
@@ -171,10 +175,8 @@ private:
src_callback_kobuki_data callbackFunction; src_callback_kobuki_data callbackFunction;
void *userData; void *userData;
bool enabledCommands; bool enabledCommands;
int parseKobukiMessage(TKobukiData &output, unsigned char *data );
int connect(char *portname); int connect(char *portname);
unsigned char *readKobukiMessage(); unsigned char *readKobukiMessage();
int checkChecksum(unsigned char *data);
//--start measurement in a new thread (reading runs in a new thread. it needs to be stopped if we want to send a request) //--start measurement in a new thread (reading runs in a new thread. it needs to be stopped if we want to send a request)
static void * KobukiProcess(void *param) static void * KobukiProcess(void *param)

261
src/C++/Driver/src/KobukiParser.cpp Normal file
View File

@@ -0,0 +1,261 @@
#include "KobukiParser.h"
#include <iostream>
int KobukiParser::parseKobukiMessage(TKobukiData &output, unsigned char *data) {
int rtrnvalue = checkChecksum(data);
if (rtrnvalue != 0) {
std::cerr << "Invalid checksum" << std::endl;
return -2;
}
int checkedValue = 1;
while (checkedValue < data[0]) {
unsigned char dataType = data[checkedValue];
unsigned char dataLength = data[checkedValue + 1];
checkedValue += 2;
switch (dataType) {
case 0x01:
if (dataLength == 0x0F) {
parseBasicData(output, data, checkedValue);
} else {
checkedValue += dataLength;
}
break;
case 0x03:
if (dataLength == 0x03) {
parseIRSensorData(output, data, checkedValue);
} else {
checkedValue += dataLength;
}
break;
case 0x04:
if (dataLength == 0x07) {
parseGyroData(output, data, checkedValue);
} else {
checkedValue += dataLength;
}
break;
case 0x05:
if (dataLength == 0x06) {
parseCliffSensorData(output, data, checkedValue);
} else {
checkedValue += dataLength;
}
break;
case 0x06:
if (dataLength == 0x02) {
parseWheelCurrentData(output, data, checkedValue);
} else {
checkedValue += dataLength;
}
break;
case 0x0A:
if (dataLength == 0x04) {
parseHardwareVersionData(output, data, checkedValue);
} else {
checkedValue += dataLength;
}
break;
case 0x0B:
if (dataLength == 0x04) {
parseFirmwareVersionData(output, data, checkedValue);
} else {
checkedValue += dataLength;
}
break;
case 0x0D:
if (dataLength % 2 == 0) {
parseXYZData(output, data, checkedValue);
} else {
checkedValue += dataLength;
}
break;
case 0x10:
if (dataLength == 0x10) {
parseDigitalAnalogInputData(output, data, checkedValue);
} else {
checkedValue += dataLength;
}
break;
case 0x13:
if (dataLength == 0x0C) {
parseUDIDData(output, data, checkedValue);
} else {
checkedValue += dataLength;
}
break;
default:
std::cerr << "Unknown data type: " << std::hex << static_cast<int>(dataType) << std::dec << std::endl;
checkedValue += dataLength;
break;
}
}
return 0;
}
int KobukiParser::parseBasicData(TKobukiData &output, unsigned char *data, int &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++;
}
int KobukiParser::parseIRSensorData(TKobukiData &output, unsigned char *data, int &checkedValue){
output.IRSensorRight = data[checkedValue];
checkedValue++;
output.IRSensorCenter = data[checkedValue];
checkedValue++;
output.IRSensorLeft = data[checkedValue];
checkedValue++;
}
int KobukiParser::parseGyroData(TKobukiData &output, unsigned char *data, int &checkedValue){
output.GyroAngle = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2;
output.GyroAngleRate = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 5; // 3 unused
}
int KobukiParser::parseCliffSensorData(TKobukiData &output, unsigned char *data, int &checkedValue){
output.CliffSensorRight = data[checkedValue];
checkedValue++;
output.CliffSensorCenter = data[checkedValue];
checkedValue++;
output.CliffSensorLeft = data[checkedValue];
checkedValue++;
}
int KobukiParser::parseWheelCurrentData(TKobukiData &output, unsigned char *data, int &checkedValue){
output.wheelCurrentLeft = data[checkedValue];
checkedValue++;
output.wheelCurrentRight = data[checkedValue];
checkedValue++;
}
int KobukiParser::parseHardwareVersionData(TKobukiData &output, unsigned char *data, int &checkedValue){
output.extraInfo.HardwareVersionPatch = data[checkedValue];
checkedValue++;
output.extraInfo.HardwareVersionMinor = data[checkedValue];
checkedValue++;
output.extraInfo.HardwareVersionMajor = data[checkedValue];
checkedValue += 2;
}
int KobukiParser::parseFirmwareVersionData(TKobukiData &output, unsigned char *data, int &checkedValue){
output.extraInfo.FirmwareVersionPatch = data[checkedValue];
checkedValue++;
output.extraInfo.FirmwareVersionMinor = data[checkedValue];
checkedValue++;
output.extraInfo.FirmwareVersionMajor = data[checkedValue];
checkedValue += 2;
}
int KobukiParser::parseExtraData(TKobukiData &output, unsigned char *data, int &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;
}
int KobukiParser::parseDigitalAnalogInputData(TKobukiData &output, unsigned char *data, int &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
}
int KobukiParser::parseUDIDData(TKobukiData &output, unsigned char *data, int &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;
}
int KobukiParser::parseXYZData(TKobukiData &output, unsigned char *data, int &checkedValue){
output.gyroData.reserve(data[checkedValue]);
output.gyroData.clear();
for (int hk = 0; hk < data[checkedValue]; 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);
}
}
int KobukiParser::checkChecksum(unsigned char *data) {
unsigned char chckSum = 0;
for (int i = 0; i < data[0] + 2; i++) {
chckSum ^= data[i];
}
return chckSum; // 0 if everything is correct, otherwise some number
}

55
src/C++/Driver/src/KobukiParser.h Normal file
View File

@@ -0,0 +1,55 @@
#ifndef KOBUKIPARSER_H //so that class cant be loaded twice
#define KOBUKIPARSER_H
#include <vector>
struct TRawGyroData {
int x, y, z;
};
struct TExtraInfo {
int HardwareVersionPatch, HardwareVersionMinor, HardwareVersionMajor;
int FirmwareVersionPatch, FirmwareVersionMinor, FirmwareVersionMajor;
int UDID0, UDID1, UDID2;
};
struct TKobukiData {
int timestamp, BumperCenter, BumperLeft, BumperRight;
int WheelDropLeft, WheelDropRight;
int CliffCenter, CliffLeft, CliffRight;
int EncoderLeft, EncoderRight;
int PWMleft, PWMright;
int ButtonPress, Charger, Battery, overCurrent;
int IRSensorRight, IRSensorCenter, IRSensorLeft;
int GyroAngle, GyroAngleRate;
int CliffSensorRight, CliffSensorCenter, CliffSensorLeft;
int wheelCurrentLeft, wheelCurrentRight;
int digitalInput, analogInputCh0, analogInputCh1, analogInputCh2, analogInputCh3;
int frameId;
TExtraInfo extraInfo; // Add extraInfo member
std::vector<TRawGyroData> gyroData; // Add gyroData member
};
class KobukiParser {
public:
int parseKobukiMessage(TKobukiData &output, unsigned char *data);
TKobukiData data;
private:
int checkChecksum(unsigned char *data);
int parseBasicData(TKobukiData &output, unsigned char *data, int &checkedValue);
int parseIRSensorData(TKobukiData &output, unsigned char *data, int &checkedValue);
int parseGyroData(TKobukiData &output, unsigned char *data, int &checkedValue);
int parseCliffSensorData(TKobukiData &output, unsigned char *data, int &checkedValue);
int parseWheelCurrentData(TKobukiData &output, unsigned char *data, int &checkedValue);
int parseHardwareVersionData(TKobukiData &output, unsigned char *data, int &checkedValue);
int parseFirmwareVersionData(TKobukiData &output, unsigned char *data, int &checkedValue);
int parseXYZData(TKobukiData &output, unsigned char *data, int &checkedValue);
int parseDigitalAnalogInputData(TKobukiData &output, unsigned char *data, int &checkedValue);
int parseUDIDData(TKobukiData &output, unsigned char *data, int &checkedValue);
int parseExtraData(TKobukiData &output, unsigned char *data, int &checkedValue);
};
#endif

18
src/C++/Driver/src/main.cpp
View File

@@ -7,26 +7,28 @@
using namespace std; using namespace std;
CKobuki robot; CKobuki robot;
int movement(); int movement();
int checkCenterCliff();
int main() int main()
{ {
unsigned char *null_ptr(0); unsigned char *null_ptr(0);
robot.startCommunication("/dev/ttyUSB0", true, null_ptr); robot.startCommunication("/dev/ttyUSB0", true, null_ptr);
usleep(1 * 1000 * 1000); // thread mv(movement);
cout << "Enter commando"; // mv.join(); //only exit once thread one is done running
thread mv(movement);
usleep(30 * 1000 * 1000); checkCenterCliff();
mv.join(); //only exit once thread one is done running
return 0;
} }
int checkCenterCliff() int checkCenterCliff()
{ {
while (true) while(true){
{ std::cout << robot.parser.data.CliffSensorCenter << endl;
std::cout << "cliffsensordata:" << robot.data.CliffSensorCenter << std::endl;
} }
} }
int movement() int movement()
{ {
int text; int text;