mirror of
https://gitlab.fdmci.hva.nl/technische-informatica-sm3/ti-projectten/rooziinuubii79.git
synced 2025-08-03 20:04:58 +00:00
Merge branch 'parser-cleanup' into 'main'
parser rewrite and put into its own class See merge request technische-informatica-sm3/ti-projectten/rooziinuubii79!1
This commit is contained in:
4
.gitignore
vendored
4
.gitignore
vendored
@@ -12,3 +12,7 @@ src/C++/Driver/vgcore.42611
|
||||
src/Socket/a.out
|
||||
src/C++/Driver/cmake_install.cmake
|
||||
src/C++/Socket/a.out
|
||||
src/C++/Driver/Makefile
|
||||
src/C++/Driver/vgcore*
|
||||
src/C++/Driver/cmake_install.cmake
|
||||
src/C++/Driver/Makefile
|
||||
|
||||
@@ -1,13 +1,12 @@
|
||||
cmake_minimum_required(VERSION 3.9)
|
||||
project(kobuki_control)
|
||||
|
||||
#set(CMAKE_CXX_STANDARD 11)
|
||||
find_package( OpenCV REQUIRED )
|
||||
|
||||
set(SOURCE_FILES
|
||||
src/KobukiParser.cpp
|
||||
src/KobukiParser.h
|
||||
src/CKobuki.cpp
|
||||
src/CKobuki.h
|
||||
src/test.cpp)
|
||||
src/main.cpp)
|
||||
|
||||
add_executable(kobuki_control ${SOURCE_FILES})
|
||||
#target_link_libraries(kobuki_control )
|
||||
@@ -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
|
||||
|
||||
@@ -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()
|
||||
@@ -4,6 +4,7 @@
|
||||
#include <cstddef>
|
||||
#include <iostream>
|
||||
|
||||
|
||||
// plot p;
|
||||
static std::vector<float> vectorX;
|
||||
static std::vector<float> vectorY;
|
||||
@@ -163,16 +164,6 @@ unsigned char *CKobuki::readKobukiMessage()
|
||||
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};
|
||||
@@ -275,16 +266,17 @@ void CKobuki::setSound(int noteinHz, int duration)
|
||||
pocet = write(HCom, &message, 9);
|
||||
}
|
||||
|
||||
void CKobuki::startCommunication(char *portname, bool CommandsEnabled,
|
||||
void *userDataL)
|
||||
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);
|
||||
pthread_result = pthread_create(&threadHandle, NULL, KobukiProcess, (void *)this);
|
||||
if (pthread_result != 0) {
|
||||
std::cerr << "Error creating thread: " << pthread_result << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
int CKobuki::measure()
|
||||
@@ -297,215 +289,215 @@ int CKobuki::measure()
|
||||
// printf("vratil null message\n");
|
||||
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
|
||||
// citat
|
||||
if (ok == 0)
|
||||
{
|
||||
loop(userData, data);
|
||||
loop(userData, parser.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 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;
|
||||
}
|
||||
// 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)
|
||||
{
|
||||
|
||||
@@ -30,93 +30,94 @@
|
||||
#include <iomanip>
|
||||
#include <chrono>
|
||||
#include <sstream>
|
||||
|
||||
#include "KobukiParser.h"
|
||||
#include "graph.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
|
||||
unsigned short x;
|
||||
unsigned short y;
|
||||
unsigned short z;
|
||||
// typedef struct
|
||||
// {
|
||||
|
||||
}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;
|
||||
// unsigned short x;
|
||||
// unsigned short y;
|
||||
// unsigned short z;
|
||||
|
||||
//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;
|
||||
// }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;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
//---basic package
|
||||
unsigned short timestamp;
|
||||
//bumpers
|
||||
bool BumperLeft;
|
||||
bool BumperCenter;
|
||||
bool BumperRight;
|
||||
//cliff
|
||||
bool CliffLeft;
|
||||
bool CliffCenter;
|
||||
bool CliffRight;
|
||||
// wheel drop
|
||||
bool WheelDropLeft;
|
||||
bool WheelDropRight;
|
||||
//wheel rotation
|
||||
unsigned short EncoderRight;
|
||||
unsigned short EncoderLeft;
|
||||
unsigned char PWMright;
|
||||
unsigned char PWMleft;
|
||||
//buttons
|
||||
unsigned char ButtonPress;// 0 no, 1 2 4 for button 0 1 2 (7 is all three)
|
||||
//power
|
||||
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;
|
||||
//---structure with data that appears only on request
|
||||
TExtraRequestData extraInfo;
|
||||
}TKobukiData;
|
||||
// //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
|
||||
// {
|
||||
// //---basic package
|
||||
// unsigned short timestamp;
|
||||
// //bumpers
|
||||
// bool BumperLeft;
|
||||
// bool BumperCenter;
|
||||
// bool BumperRight;
|
||||
// //cliff
|
||||
// bool CliffLeft;
|
||||
// bool CliffCenter;
|
||||
// bool CliffRight;
|
||||
// // wheel drop
|
||||
// bool WheelDropLeft;
|
||||
// bool WheelDropRight;
|
||||
// //wheel rotation
|
||||
// unsigned short EncoderRight;
|
||||
// unsigned short EncoderLeft;
|
||||
// unsigned char PWMright;
|
||||
// unsigned char PWMleft;
|
||||
// //buttons
|
||||
// unsigned char ButtonPress;// 0 no, 1 2 4 for button 0 1 2 (7 is all three)
|
||||
// //power
|
||||
// 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;
|
||||
// //---structure with data that appears only on request
|
||||
// TExtraRequestData extraInfo;
|
||||
// }TKobukiData;
|
||||
|
||||
|
||||
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
|
||||
{
|
||||
public:
|
||||
|
||||
CKobuki() {
|
||||
stopVlakno = 0;
|
||||
std::cout << "kobuki instantiated" << std::endl;
|
||||
@@ -158,12 +160,14 @@ public:
|
||||
void doRotation(long double th);
|
||||
void goToXy(long double xx, long double yy);
|
||||
std::ofstream odometry_log;
|
||||
TKobukiData data;
|
||||
KobukiParser parser;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
private:
|
||||
|
||||
int HCom;
|
||||
pthread_t threadHandle; // handle to the thread
|
||||
int threadID; // thread id
|
||||
@@ -171,10 +175,8 @@ private:
|
||||
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);
|
||||
|
||||
//--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)
|
||||
|
||||
261
src/C++/Driver/src/KobukiParser.cpp
Normal file
261
src/C++/Driver/src/KobukiParser.cpp
Normal 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
55
src/C++/Driver/src/KobukiParser.h
Normal 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
|
||||
@@ -7,26 +7,28 @@
|
||||
using namespace std;
|
||||
CKobuki robot;
|
||||
int movement();
|
||||
int checkCenterCliff();
|
||||
|
||||
int main()
|
||||
{
|
||||
unsigned char *null_ptr(0);
|
||||
robot.startCommunication("/dev/ttyUSB0", true, null_ptr);
|
||||
usleep(1 * 1000 * 1000);
|
||||
cout << "Enter commando";
|
||||
thread mv(movement);
|
||||
usleep(30 * 1000 * 1000);
|
||||
mv.join(); //only exit once thread one is done running
|
||||
// thread mv(movement);
|
||||
// mv.join(); //only exit once thread one is done running
|
||||
|
||||
checkCenterCliff();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int checkCenterCliff()
|
||||
{
|
||||
while (true)
|
||||
{
|
||||
std::cout << "cliffsensordata:" << robot.data.CliffSensorCenter << std::endl;
|
||||
while(true){
|
||||
std::cout << robot.parser.data.CliffSensorCenter << endl;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
int movement()
|
||||
{
|
||||
int text;
|
||||
|
||||
Reference in New Issue
Block a user