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 'main' of ssh://gitlab.fdmci.hva.nl/technische-informatica-sm3/ti-projectten/rooziinuubii79
This commit is contained in:
ishak jmilou.ishak
9
.gitignore
vendored
9
.gitignore
vendored
@@ -2,3 +2,12 @@
|
||||
src/C++/main.o
|
||||
src/C++/progr
|
||||
.vscode/settings.json
|
||||
.vscode
|
||||
src/C++/Driver/odometry.txt
|
||||
src/C++/Driver/CMakeCache.txt
|
||||
src/C++/Driver/kobuki_control
|
||||
src/C++/Driver/cmake_install.cmake
|
||||
src/C++/Driver/vgcore.42436
|
||||
src/C++/Driver/vgcore.42611
|
||||
src/Socket/a.out
|
||||
src/C++/Driver/cmake_install.cmake
|
||||
|
||||
31
docs/Infrastructure/infrastructure.md
Normal file
31
docs/Infrastructure/infrastructure.md
Normal file
@@ -0,0 +1,31 @@
|
||||
# Infrastructure
|
||||
|
||||
```mermaid
|
||||
classDiagram
|
||||
|
||||
VirtualMachine <--> RPI
|
||||
|
||||
namespace Server {
|
||||
class VirtualMachine {
|
||||
+Apache()
|
||||
+WebsocketServer()
|
||||
+MariaDB()
|
||||
Python/C++
|
||||
Database
|
||||
Website
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
namespace kobuki {
|
||||
class RPI {
|
||||
Receiver
|
||||
Sensors
|
||||
C++
|
||||
+WebsocketClient()
|
||||
+Kobuki()
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
```
|
||||
10
docs/Infrastructure/taken.md
Normal file
10
docs/Infrastructure/taken.md
Normal file
@@ -0,0 +1,10 @@
|
||||
# Taken
|
||||
|
||||
|
||||
* [ ] 2 way socket schrijven voor pi en server (maybe 1 taal maybe 2 talen)
|
||||
- [ ] Data van kobuki omzetten naar json en versturen naar server via socket
|
||||
- [ ] Website bouwen en data er op displayen
|
||||
- [ ] De kobuki vanaf de website besturen
|
||||
* [ ] Zorg er voor dat de pi de kobuki kan besturen
|
||||
* [ ] Zorg er voor dat de website naar de server een commando kan sturen
|
||||
* [ ] Zorg er voor dat de server het commando naar de pi kan sturen
|
||||
33
src/C++/Driver/.gitignore
vendored
Normal file
33
src/C++/Driver/.gitignore
vendored
Normal file
@@ -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/*
|
||||
|
||||
13
src/C++/Driver/CMakeLists.txt
Normal file
13
src/C++/Driver/CMakeLists.txt
Normal file
@@ -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 )
|
||||
21
src/C++/Driver/LICENSE
Normal file
21
src/C++/Driver/LICENSE
Normal file
@@ -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.
|
||||
208
src/C++/Driver/Makefile
Normal file
208
src/C++/Driver/Makefile
Normal file
@@ -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
|
||||
|
||||
62
src/C++/Driver/cmake_install.cmake
Normal file
62
src/C++/Driver/cmake_install.cmake
Normal file
@@ -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 "DEBUG")
|
||||
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()
|
||||
740
src/C++/Driver/src/CKobuki.cpp
Executable file
740
src/C++/Driver/src/CKobuki.cpp
Executable file
@@ -0,0 +1,740 @@
|
||||
#include "CKobuki.h"
|
||||
#include "termios.h"
|
||||
#include "errno.h"
|
||||
#include <cstddef>
|
||||
#include <iostream>
|
||||
|
||||
// plot p;
|
||||
static std::vector<float> vectorX;
|
||||
static std::vector<float> vectorY;
|
||||
static std::vector<float> vectorGyroTheta;
|
||||
|
||||
// obsluha tty pod unixom
|
||||
int set_interface_attribs2 (int fd, int speed, int parity)
|
||||
{
|
||||
struct termios tty;
|
||||
memset (&tty, 0, sizeof tty);
|
||||
if (tcgetattr (fd, &tty) != 0)
|
||||
{
|
||||
printf ("error %d from tcgetattr", errno);
|
||||
return -1;
|
||||
}
|
||||
|
||||
cfsetospeed (&tty, speed);
|
||||
cfsetispeed (&tty, speed);
|
||||
|
||||
tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8; // 8-bit chars
|
||||
// disable IGNBRK for mismatched speed tests; otherwise receive break
|
||||
// as \000 chars
|
||||
//tty.c_iflag &= ~IGNBRK; // disable break processing
|
||||
tty.c_lflag = 0; // no signaling chars, no echo,
|
||||
// no canonical processing
|
||||
tty.c_oflag = 0; // no remapping, no delays
|
||||
tty.c_cc[VMIN] = 0; // read doesn't block
|
||||
tty.c_cc[VTIME] = 5; // 0.5 seconds read timeout
|
||||
|
||||
tty.c_iflag &= ~(IGNBRK | INLCR | ICRNL | IXON | IXOFF | IXANY); // shut off xon/xoff ctrl
|
||||
|
||||
tty.c_cflag |= (CLOCAL | CREAD);// ignore modem controls,
|
||||
// enable reading
|
||||
tty.c_cflag &= ~(PARENB | PARODD); // shut off parity
|
||||
tty.c_cflag |= parity;
|
||||
tty.c_cflag &= ~CSTOPB;
|
||||
tty.c_cflag &= ~CRTSCTS;
|
||||
|
||||
if (tcsetattr (fd, TCSANOW, &tty) != 0)
|
||||
{
|
||||
printf ("error %d from tcsetattr", errno);
|
||||
return -1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
void set_blocking2 (int fd, int should_block)
|
||||
{
|
||||
struct termios tty;
|
||||
memset (&tty, 0, sizeof tty);
|
||||
if (tcgetattr (fd, &tty) != 0)
|
||||
{
|
||||
printf ("error %d from tggetattr", errno);
|
||||
return;
|
||||
}
|
||||
|
||||
tty.c_cc[VMIN] = should_block ? 1 : 0;
|
||||
tty.c_cc[VTIME] = 5; // 0.5 seconds read timeout
|
||||
|
||||
if (tcsetattr (fd, TCSANOW, &tty) != 0)
|
||||
printf ("error %d setting term attributes", errno);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
int CKobuki::connect(char * comportT)
|
||||
{
|
||||
HCom= open(comportT,O_RDWR|O_NOCTTY|O_NONBLOCK);
|
||||
|
||||
if ( HCom== -1 )
|
||||
{
|
||||
printf("Kobuki nepripojeny\n");
|
||||
return HCom;
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
set_interface_attribs2 (HCom, B115200, 0); // set speed to 115,200 bps, 8n1 (no parity)
|
||||
set_blocking2 (HCom, 0); // set no blocking
|
||||
/* struct termios settings;
|
||||
tcgetattr(HCom, &settings);
|
||||
|
||||
cfsetospeed(&settings, B115200); // baud rate
|
||||
settings.c_cflag &= ~PARENB; // no parity
|
||||
settings.c_cflag &= ~CSTOPB; // 1 stop bit
|
||||
settings.c_cflag &= ~CSIZE;
|
||||
settings.c_cflag |= CS8 | CLOCAL; // 8 bits
|
||||
settings.c_lflag &= ~ICANON; // canonical mode
|
||||
settings.c_cc[VTIME]=1;
|
||||
settings.c_oflag &= ~OPOST; // raw output
|
||||
|
||||
tcsetattr(HCom, TCSANOW, &settings); // apply the settings*/
|
||||
tcflush(HCom, TCOFLUSH);
|
||||
|
||||
|
||||
printf("Kobuki pripojeny\n");
|
||||
return HCom;
|
||||
}
|
||||
}
|
||||
|
||||
unsigned char * CKobuki::readKobukiMessage()
|
||||
{
|
||||
unsigned char buffer[1];
|
||||
ssize_t Pocet;
|
||||
buffer[0] = 0;
|
||||
unsigned char * null_buffer(0);
|
||||
//citame kym nezachytime zaciatok spravy
|
||||
do {
|
||||
Pocet=read(HCom,buffer,1);
|
||||
} while (buffer[0] != 0xAA);
|
||||
//mame zaciatok spravy (asi)
|
||||
if (Pocet == 1 && buffer[0] == 0xAA)
|
||||
{
|
||||
//citame dalsi byte
|
||||
do {
|
||||
|
||||
Pocet=read(HCom,buffer,1);
|
||||
|
||||
} while (Pocet != 1); // na linuxe -1 na windowse 0
|
||||
|
||||
|
||||
|
||||
//a ak je to druhy byte hlavicky
|
||||
if (Pocet == 1 && buffer[0] == 0x55)
|
||||
{
|
||||
// precitame dlzku
|
||||
Pocet=read(HCom,buffer,1);
|
||||
|
||||
// ReadFile(hCom, buffer, 1, &Pocet, NULL);
|
||||
if (Pocet == 1)
|
||||
{
|
||||
//mame dlzku.. nastavime vektor a precitame ho cely
|
||||
int readLenght = buffer[0];
|
||||
unsigned char *outputBuffer = (unsigned char*)calloc(readLenght+4,sizeof(char));
|
||||
outputBuffer[0] = buffer[0];
|
||||
int pct = 0;
|
||||
|
||||
do
|
||||
{
|
||||
Pocet = 0;
|
||||
int readpoc = (readLenght+1 - pct);
|
||||
Pocet=read(HCom,outputBuffer+1+pct,readpoc);
|
||||
|
||||
pct = pct + (Pocet == -1 ? 0 : Pocet);
|
||||
} while (pct != (readLenght+1 ));
|
||||
|
||||
// tu si mozeme ceknut co chodi zo serial intefejsu Kobukiho
|
||||
// for(int i=0;i<outputBuffer[0]+2;i++)
|
||||
// {
|
||||
// printf("%x ",outputBuffer[i]);
|
||||
// }
|
||||
|
||||
return outputBuffer;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return null_buffer;
|
||||
}
|
||||
|
||||
int CKobuki::checkChecksum(unsigned char * data)
|
||||
{//najprv hlavicku
|
||||
unsigned char chckSum = 0;
|
||||
for (int i = 0; i < data[0]+2; i++)
|
||||
{
|
||||
chckSum ^= data[i];
|
||||
}
|
||||
return chckSum;//0 ak je vsetko v poriadku,inak nejake cislo
|
||||
}
|
||||
|
||||
void CKobuki::setLed(int led1, int led2)
|
||||
{
|
||||
unsigned char message[8] = {0xaa,0x55,0x04,0x0c,0x02,0x00,(unsigned char)((led1+led2*4)%256),0x00};
|
||||
message[7] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6];
|
||||
uint32_t pocet;
|
||||
pocet=write(HCom,&message,8);
|
||||
}
|
||||
|
||||
// tato funkcia nema moc sama o sebe vyznam, payload o tom, ze maju byt externe napajania aktivne musi byt aj tak v kazdej sprave...
|
||||
void CKobuki::setPower(int value){
|
||||
if (value == 1) {
|
||||
unsigned char message[8] = {0xaa,0x55,0x04,0x0C,0x02,0xf0,0x00,0xAF};
|
||||
uint32_t pocet;
|
||||
pocet = write(HCom,&message,8);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void CKobuki::setTranslationSpeed(int mmpersec)
|
||||
{
|
||||
unsigned char message[14] = { 0xaa,0x55,0x0A,0x0c,0x02,0xf0,0x00,0x01,0x04,mmpersec%256,mmpersec>>8,0x00,0x00, 0x00 };
|
||||
message[13] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^ message[7] ^ message[8] ^ message[9] ^ message[10] ^ message[11] ^ message[12];
|
||||
|
||||
uint32_t pocet;
|
||||
pocet=write(HCom,&message,14);
|
||||
|
||||
}
|
||||
|
||||
void CKobuki::setRotationSpeed(double radpersec)
|
||||
{
|
||||
int speedvalue = radpersec * 230.0f / 2.0f;
|
||||
unsigned char message[14] = { 0xaa,0x55,0x0A,0x0c,0x02,0xf0,0x00,0x01,0x04,speedvalue % 256,speedvalue >>8,0x01,0x00, 0x00 };
|
||||
message[13] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^ message[7] ^ message[8] ^ message[9] ^ message[10] ^ message[11] ^ message[12];
|
||||
|
||||
uint32_t pocet;
|
||||
pocet=write(HCom,&message,14);
|
||||
}
|
||||
|
||||
void CKobuki::setArcSpeed(int mmpersec, int radius)
|
||||
{
|
||||
if (radius == 0) {
|
||||
setTranslationSpeed(mmpersec);
|
||||
return;
|
||||
}
|
||||
|
||||
int speedvalue = mmpersec * ((radius + (radius>0? 230:-230) )/ 2 ) / radius;
|
||||
unsigned char message[14] = { 0xaa,0x55,0x0A,0x0c,0x02,0xf0,0x00,0x01,0x04,speedvalue % 256,speedvalue >>8,radius % 256,radius >>8, 0x00 };
|
||||
message[13] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^ message[7] ^ message[8] ^ message[9] ^ message[10] ^ message[11] ^ message[12];
|
||||
uint32_t pocet;
|
||||
pocet=write(HCom,&message,14);
|
||||
}
|
||||
|
||||
void CKobuki::setSound(int noteinHz, int duration)
|
||||
{
|
||||
int notevalue = floor((double)1.0 / ((double)noteinHz*0.00000275) + 0.5);
|
||||
unsigned char message[9] = { 0xaa,0x55,0x05,0x03,0x03,notevalue%256,notevalue>>8,duration%256,0x00 };
|
||||
message[8] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^ message[7];
|
||||
|
||||
uint32_t pocet;
|
||||
pocet=write(HCom,&message,9);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void CKobuki::startCommunication(char * portname, bool CommandsEnabled, void *userDataL)
|
||||
{
|
||||
connect(portname);
|
||||
enableCommands(CommandsEnabled);
|
||||
userData = userDataL;
|
||||
|
||||
int pthread_result;
|
||||
pthread_result = pthread_create(&threadHandle,NULL,KobukiProcess,(void *)this);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
int CKobuki::measure()
|
||||
{
|
||||
while (stopVlakno==0)
|
||||
{
|
||||
unsigned char *message = readKobukiMessage();
|
||||
if (message == NULL)
|
||||
{
|
||||
// printf("vratil null message\n");
|
||||
continue;
|
||||
}
|
||||
int ok=parseKobukiMessage(data,message);
|
||||
|
||||
//maximalne moze trvat callback funkcia 20 ms, ak by trvala viac, nestihame citat
|
||||
if (ok == 0)
|
||||
{
|
||||
loop(userData, data);
|
||||
}
|
||||
free(message);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char * data)
|
||||
{
|
||||
int rtrnvalue = checkChecksum(data);
|
||||
//ak je zly checksum,tak kaslat na to
|
||||
if (rtrnvalue != 0)
|
||||
return -2;
|
||||
|
||||
int checkedValue = 1;
|
||||
//kym neprejdeme celu dlzku
|
||||
while (checkedValue < data[0])
|
||||
{
|
||||
//basic data subload
|
||||
if (data[checkedValue] == 0x01)
|
||||
{
|
||||
checkedValue++;
|
||||
if (data[checkedValue ] != 0x0F)
|
||||
return -1;
|
||||
checkedValue++;
|
||||
output.timestamp = data[checkedValue + 1] * 256 + data[checkedValue];
|
||||
checkedValue += 2;
|
||||
output.BumperCenter = data[checkedValue ] && 0x02;
|
||||
output.BumperLeft = data[checkedValue] && 0x04;
|
||||
output.BumperRight = data[checkedValue] && 0x01;
|
||||
checkedValue++;
|
||||
output.WheelDropLeft= data[checkedValue] && 0x02;
|
||||
output.WheelDropRight = data[checkedValue] && 0x01;
|
||||
checkedValue++;
|
||||
output.CliffCenter = data[checkedValue] && 0x02;
|
||||
output.CliffLeft = data[checkedValue] && 0x04;
|
||||
output.CliffRight = data[checkedValue] && 0x01;
|
||||
checkedValue++;
|
||||
output.EncoderLeft = data[checkedValue + 1] * 256 + data[checkedValue];
|
||||
checkedValue += 2;
|
||||
output.EncoderRight = data[checkedValue + 1] * 256 + data[checkedValue];
|
||||
checkedValue += 2;
|
||||
output.PWMleft = data[checkedValue] ;
|
||||
checkedValue++;
|
||||
output.PWMright = data[checkedValue] ;
|
||||
checkedValue++;
|
||||
output.ButtonPress = data[checkedValue];
|
||||
checkedValue++;
|
||||
output.Charger = data[checkedValue];
|
||||
checkedValue++;
|
||||
output.Battery = data[checkedValue];
|
||||
checkedValue++;
|
||||
output.overCurrent = data[checkedValue];
|
||||
checkedValue++;
|
||||
}
|
||||
else if (data[checkedValue] == 0x03)
|
||||
{
|
||||
checkedValue++;
|
||||
if (data[checkedValue] != 0x03)
|
||||
return -3;
|
||||
checkedValue++;
|
||||
output.IRSensorRight = data[checkedValue];
|
||||
checkedValue++;
|
||||
output.IRSensorCenter = data[checkedValue];
|
||||
checkedValue++;
|
||||
output.IRSensorLeft = data[checkedValue];
|
||||
checkedValue++;
|
||||
}
|
||||
else if (data[checkedValue] == 0x04)
|
||||
{
|
||||
checkedValue++;
|
||||
if (data[checkedValue] != 0x07)
|
||||
return -4;
|
||||
checkedValue++;
|
||||
output.GyroAngle = data[checkedValue + 1] * 256 + data[checkedValue];
|
||||
checkedValue += 2;
|
||||
output.GyroAngleRate = data[checkedValue + 1] * 256 + data[checkedValue];
|
||||
checkedValue += 5;//3 unsued
|
||||
}
|
||||
else if (data[checkedValue] == 0x05)
|
||||
{
|
||||
checkedValue++;
|
||||
if (data[checkedValue] != 0x06)
|
||||
return -5;
|
||||
checkedValue++;
|
||||
output.CliffSensorRight = data[checkedValue + 1] * 256 + data[checkedValue];
|
||||
checkedValue += 2;
|
||||
output.CliffSensorCenter = data[checkedValue + 1] * 256 + data[checkedValue];
|
||||
checkedValue += 2;
|
||||
output.CliffSensorLeft = data[checkedValue + 1] * 256 + data[checkedValue];
|
||||
checkedValue += 2;
|
||||
}
|
||||
else if (data[checkedValue] == 0x06)
|
||||
{
|
||||
checkedValue++;
|
||||
if (data[checkedValue] != 0x02)
|
||||
return -6;
|
||||
checkedValue++;
|
||||
output.wheelCurrentLeft = data[checkedValue];
|
||||
checkedValue ++;
|
||||
output.wheelCurrentRight =data[checkedValue];
|
||||
checkedValue ++;
|
||||
|
||||
}
|
||||
else if (data[checkedValue] == 0x0A)
|
||||
{
|
||||
checkedValue++;
|
||||
if (data[checkedValue] != 0x04)
|
||||
return -7;
|
||||
checkedValue++;
|
||||
output.extraInfo.HardwareVersionPatch = data[checkedValue];
|
||||
checkedValue++;
|
||||
output.extraInfo.HardwareVersionMinor = data[checkedValue];
|
||||
checkedValue++;
|
||||
output.extraInfo.HardwareVersionMajor = data[checkedValue];
|
||||
checkedValue += 2;
|
||||
|
||||
}
|
||||
else if (data[checkedValue] == 0x0B)
|
||||
{
|
||||
checkedValue++;
|
||||
if (data[checkedValue] != 0x04)
|
||||
return -8;
|
||||
checkedValue++;
|
||||
output.extraInfo.FirmwareVersionPatch = data[checkedValue];
|
||||
checkedValue++;
|
||||
output.extraInfo.FirmwareVersionMinor = data[checkedValue];
|
||||
checkedValue++;
|
||||
output.extraInfo.FirmwareVersionMajor = data[checkedValue];
|
||||
checkedValue += 2;
|
||||
|
||||
}
|
||||
else if (data[checkedValue] == 0x0D)
|
||||
{
|
||||
checkedValue++;
|
||||
if (data[checkedValue]%2 !=0)
|
||||
return -9;
|
||||
checkedValue++;
|
||||
output.frameId = data[checkedValue];
|
||||
checkedValue++;
|
||||
int howmanyFrames = data[checkedValue]/3;
|
||||
checkedValue++;
|
||||
output.gyroData.reserve(howmanyFrames);
|
||||
output.gyroData.clear();
|
||||
for (int hk = 0; hk < howmanyFrames; hk++)
|
||||
{
|
||||
TRawGyroData temp;
|
||||
temp.x = data[checkedValue + 1] * 256 + data[checkedValue];
|
||||
checkedValue += 2;
|
||||
temp.y = data[checkedValue + 1] * 256 + data[checkedValue];
|
||||
checkedValue += 2;
|
||||
temp.z = data[checkedValue + 1] * 256 + data[checkedValue];
|
||||
checkedValue += 2;
|
||||
output.gyroData.push_back(temp);
|
||||
}
|
||||
}
|
||||
else if (data[checkedValue] == 0x10)
|
||||
{
|
||||
checkedValue++;
|
||||
if (data[checkedValue] != 0x10)
|
||||
return -10;
|
||||
checkedValue++;
|
||||
output.digitalInput = data[checkedValue + 1] * 256 + data[checkedValue];
|
||||
checkedValue += 2;
|
||||
output.analogInputCh0 = data[checkedValue + 1] * 256 + data[checkedValue];
|
||||
checkedValue += 2;
|
||||
output.analogInputCh1 = data[checkedValue + 1] * 256 + data[checkedValue];
|
||||
checkedValue += 2;
|
||||
output.analogInputCh2 = data[checkedValue + 1] * 256 + data[checkedValue];
|
||||
checkedValue += 2;
|
||||
output.analogInputCh3 = data[checkedValue + 1] * 256 + data[checkedValue];
|
||||
checkedValue += 8;//2+6
|
||||
|
||||
|
||||
}
|
||||
else if (data[checkedValue] == 0x13)
|
||||
{
|
||||
checkedValue++;
|
||||
if (data[checkedValue] != 0x0C)
|
||||
return -11;
|
||||
checkedValue++;
|
||||
output.extraInfo.UDID0 = data[checkedValue + 3] * 256*256*256+ data[checkedValue + 2] * 256*256+ data[checkedValue + 1] * 256 + data[checkedValue];
|
||||
checkedValue += 4;
|
||||
output.extraInfo.UDID1 = data[checkedValue + 3] * 256 * 256 * 256 + data[checkedValue + 2] * 256 * 256 +data[checkedValue + 1] * 256 + data[checkedValue];
|
||||
checkedValue += 4;
|
||||
output.extraInfo.UDID2 = data[checkedValue + 3] * 256 * 256 * 256 + data[checkedValue + 2] * 256 * 256 +data[checkedValue + 1] * 256 + data[checkedValue];
|
||||
checkedValue += 4;
|
||||
}
|
||||
else
|
||||
{
|
||||
checkedValue++;
|
||||
checkedValue += data[checkedValue] + 1;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
long double CKobuki::gyroToRad(signed short GyroAngle) {
|
||||
|
||||
long double ret;
|
||||
if (GyroAngle < 0) {
|
||||
ret = GyroAngle + 36000;
|
||||
}
|
||||
else {
|
||||
ret = GyroAngle;
|
||||
}
|
||||
return (long double) ret*PI/18000.0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
long CKobuki::loop(void *user_data, TKobukiData &Kobuki_data) {
|
||||
if (iterationCount == 0) {
|
||||
prevLeftEncoder = Kobuki_data.EncoderLeft;
|
||||
prevRightEncoder = Kobuki_data.EncoderRight;
|
||||
prevTimestamp = Kobuki_data.timestamp;
|
||||
prevGyroTheta = gyroToRad(Kobuki_data.GyroAngle);
|
||||
iterationCount++;
|
||||
}
|
||||
|
||||
int dLeft;
|
||||
if (abs(Kobuki_data.EncoderLeft - prevLeftEncoder) > 32000) {
|
||||
dLeft = Kobuki_data.EncoderLeft - prevLeftEncoder + (Kobuki_data.EncoderLeft > prevLeftEncoder ? -65536 : +65536);
|
||||
}
|
||||
else {
|
||||
dLeft = Kobuki_data.EncoderLeft - prevLeftEncoder;
|
||||
}
|
||||
|
||||
int dRight;
|
||||
if (abs(Kobuki_data.EncoderRight - prevRightEncoder) > 32000) {
|
||||
dRight = Kobuki_data.EncoderRight - prevRightEncoder + (Kobuki_data.EncoderRight > prevRightEncoder ? -65536 : +65536);
|
||||
}
|
||||
else {
|
||||
dRight = Kobuki_data.EncoderRight - prevRightEncoder;
|
||||
}
|
||||
|
||||
long double dGyroTheta = prevGyroTheta - gyroToRad(Kobuki_data.GyroAngle);
|
||||
|
||||
if (dGyroTheta > PI) {
|
||||
dGyroTheta -= 2*PI;
|
||||
}
|
||||
if (dGyroTheta < -1*PI) {
|
||||
dGyroTheta += 2*PI;
|
||||
}
|
||||
|
||||
gyroTheta += dGyroTheta;
|
||||
|
||||
uint16_t dTimestamp = Kobuki_data.timestamp - prevTimestamp;
|
||||
|
||||
long double mLeft = dLeft*tickToMeter;
|
||||
long double mRight = dRight*tickToMeter;
|
||||
|
||||
if (mLeft == mRight) {
|
||||
x = x + mRight;
|
||||
} else {
|
||||
x = x + (b*(mRight+mLeft))/(2*(mRight-mLeft))*(sin((mRight-mLeft)/b + theta) - sin(theta));
|
||||
y = y + (b*(mRight+mLeft))/(2*(mRight-mLeft))*(cos((mRight-mLeft)/b + theta) - cos(theta));
|
||||
theta = (mRight-mLeft)/b + theta;
|
||||
}
|
||||
|
||||
displacement = (mRight + mLeft)/2;
|
||||
integratedGyroTheta = integratedGyroTheta + dGyroTheta;
|
||||
gx = gx + displacement * cos(integratedGyroTheta + dGyroTheta / 2);
|
||||
|
||||
gy = gy + displacement * sin(integratedGyroTheta + dGyroTheta / 2);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
totalLeft +=dLeft;
|
||||
totalRight +=dRight;
|
||||
|
||||
// ak je suma novej a predchadzajucej vacsia ako 65536 tak to pretieklo?
|
||||
directionL = (prevLeftEncoder < Kobuki_data.EncoderLeft ? 1 : -1);
|
||||
directionR = (prevRightEncoder < Kobuki_data.EncoderRight ? 1 : -1);
|
||||
dTimestamp = (Kobuki_data.timestamp < prevTimestamp ? prevTimestamp - Kobuki_data.timestamp + 65536 : dTimestamp);
|
||||
|
||||
|
||||
prevLeftEncoder = Kobuki_data.EncoderLeft;
|
||||
prevRightEncoder = Kobuki_data.EncoderRight;
|
||||
prevTimestamp = Kobuki_data.timestamp;
|
||||
prevGyroTheta = gyroToRad(Kobuki_data.GyroAngle);
|
||||
|
||||
|
||||
// std::cout << "X: " << x
|
||||
// << " Y: " << y
|
||||
// << " Theta: " << theta
|
||||
// << "Gyro theta:" << gyroTheta
|
||||
// << std::endl;
|
||||
|
||||
|
||||
static long counter = 0;
|
||||
|
||||
vectorX.push_back(gx);
|
||||
vectorY.push_back(gy);
|
||||
vectorGyroTheta.push_back(gyroTheta);
|
||||
|
||||
// if (counter % 100 == 0) {
|
||||
// p.plot_data(vectorY, vectorX);
|
||||
// }
|
||||
counter++;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
// povie kobukimu ze ma ist niekolko metrov dopredu alebo dozadu, rozhoduje znamienko
|
||||
// funkcia kompenzuje chodenie rovno pomocou regulatora, interne vyuziva setArcSpeed a
|
||||
// ako spatnu vazbu pouziva data z enkoderov
|
||||
void CKobuki::goStraight(long double distance){
|
||||
long double u_translation = 0; // riadena velicina, rychlost robota pri pohybe
|
||||
long double w_translation = distance; // pozadovana hodnota
|
||||
|
||||
// parametre regulatora
|
||||
long double Kp_translation = 4500;
|
||||
long double e_translation = 0;
|
||||
int upper_thresh_translation = 600;
|
||||
int lower_thresh_translation = 40;
|
||||
int translation_start_gain = 20;
|
||||
|
||||
long double u_rotation = 0; // riadena velicina
|
||||
long double w_rotation = 0;
|
||||
long double Kp_rotation = 57;
|
||||
long double e_rotation = 0;
|
||||
|
||||
x = 0;
|
||||
y = 0;
|
||||
theta = 0;
|
||||
|
||||
long i = 5;
|
||||
|
||||
while (fabs(x - w_translation) > 0.005 && x<w_translation) {
|
||||
e_translation = w_translation - x;
|
||||
u_translation = Kp_translation * e_translation;
|
||||
|
||||
e_rotation = w_rotation - theta;
|
||||
if (!e_rotation == 0) u_rotation = Kp_rotation / e_rotation;
|
||||
|
||||
|
||||
// limit translation speed
|
||||
if (u_translation > upper_thresh_translation)
|
||||
u_translation = upper_thresh_translation;
|
||||
if (u_translation < lower_thresh_translation)
|
||||
u_translation = lower_thresh_translation;
|
||||
|
||||
// rewrite starting speed with line
|
||||
if (i < u_translation) {
|
||||
u_translation = i;
|
||||
}
|
||||
|
||||
if (fabs(u_rotation) > 32767) {
|
||||
u_rotation = -32767;
|
||||
}
|
||||
|
||||
if (u_rotation == 0) {
|
||||
u_rotation = -32767;
|
||||
}
|
||||
|
||||
this->setArcSpeed(u_translation, u_rotation);
|
||||
|
||||
usleep(25*1000);
|
||||
// increment starting speed
|
||||
i = i + translation_start_gain;
|
||||
}
|
||||
this->setTranslationSpeed(0);
|
||||
}
|
||||
|
||||
|
||||
|
||||
/// the method performs the rotation, it rotates using the regulator, the gyroscope serves as feedback,
|
||||
/// because it is much more accurate than encoders
|
||||
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;
|
||||
}
|
||||
|
||||
225
src/C++/Driver/src/CKobuki.h
Executable file
225
src/C++/Driver/src/CKobuki.h
Executable file
@@ -0,0 +1,225 @@
|
||||
//#pragma once
|
||||
////*************************************************************************************
|
||||
////*************************************************************************************
|
||||
//// autor Martin Dekan a Peter Beno mail: dekdekan@gmail.com, peter.beno@stuba.sk
|
||||
////-------------------------------------------------------------------------------------
|
||||
//// co to je:
|
||||
//// trieda na pracu s robotom kobuki. mala by mat implementovane citanie dat
|
||||
//// a posielanie prikazov...
|
||||
//// neobsahuje ziadnu logiku co s datami robit, to je na userovi aby spravil v callback funkcii
|
||||
// jedna sa vlastne len o implementaciu komunikacie s hardwareom, driver
|
||||
////*************************************************************************************
|
||||
////*************************************************************************************
|
||||
#ifndef KOBUKI_CLASS_123456789
|
||||
#define KOBUKI_CLASS_123456789
|
||||
#define PI 3.141592653589793238462643383279502884L /* pi */
|
||||
#define MS_INSTRUCTION_DELAY 25
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <vector>
|
||||
#include "pthread.h"
|
||||
#include "unistd.h"
|
||||
#include "fcntl.h"
|
||||
#include "string.h"
|
||||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
#include <cmath>
|
||||
#include <iomanip>
|
||||
#include <chrono>
|
||||
#include <sstream>
|
||||
|
||||
#include "graph.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
|
||||
unsigned short x;
|
||||
unsigned short y;
|
||||
unsigned short z;
|
||||
|
||||
}TRawGyroData;
|
||||
typedef struct
|
||||
{
|
||||
//Hardware Version
|
||||
unsigned char HardwareVersionMajor;
|
||||
unsigned char HardwareVersionMinor;
|
||||
unsigned char HardwareVersionPatch;
|
||||
//Firmware Version
|
||||
unsigned char FirmwareVersionMajor;
|
||||
unsigned char FirmwareVersionMinor;
|
||||
unsigned char FirmwareVersionPatch;
|
||||
|
||||
//Unique Device IDentifier(UDID)
|
||||
unsigned int UDID0;
|
||||
unsigned int UDID1;
|
||||
unsigned int UDID2;
|
||||
//Controller Info
|
||||
unsigned char PIDtype;
|
||||
unsigned int PIDgainP;
|
||||
unsigned int PIDgainI;
|
||||
unsigned int PIDgainD;
|
||||
}TExtraRequestData;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
//---zakladny balik
|
||||
unsigned short timestamp;
|
||||
//narazniky
|
||||
bool BumperLeft;
|
||||
bool BumperCenter;
|
||||
bool BumperRight;
|
||||
//cliff
|
||||
bool CliffLeft;
|
||||
bool CliffCenter;
|
||||
bool CliffRight;
|
||||
// padnutie kolies
|
||||
bool WheelDropLeft;
|
||||
bool WheelDropRight;
|
||||
//tocenie kolies
|
||||
unsigned short EncoderRight;
|
||||
unsigned short EncoderLeft;
|
||||
unsigned char PWMright;
|
||||
unsigned char PWMleft;
|
||||
//gombiky
|
||||
unsigned char ButtonPress;// 0 nie, 1 2 4 pre button 0 1 2 (7 je ze vsetky tri)
|
||||
//napajanie
|
||||
unsigned char Charger;
|
||||
unsigned char Battery;
|
||||
unsigned char overCurrent;
|
||||
//---docking ir
|
||||
unsigned char IRSensorRight;
|
||||
unsigned char IRSensorCenter;
|
||||
unsigned char IRSensorLeft;
|
||||
//---Inertial Sensor Data
|
||||
signed short GyroAngle;
|
||||
unsigned short GyroAngleRate;
|
||||
//---Cliff Sensor Data
|
||||
unsigned short CliffSensorRight;
|
||||
unsigned short CliffSensorCenter;
|
||||
unsigned short CliffSensorLeft;
|
||||
//---Current
|
||||
unsigned char wheelCurrentLeft;
|
||||
unsigned char wheelCurrentRight;
|
||||
//---Raw Data Of 3D Gyro
|
||||
unsigned char frameId;
|
||||
std::vector<TRawGyroData> gyroData;
|
||||
//---General Purpose Input
|
||||
unsigned short digitalInput;
|
||||
unsigned short analogInputCh0;
|
||||
unsigned short analogInputCh1;
|
||||
unsigned short analogInputCh2;
|
||||
unsigned short analogInputCh3;
|
||||
//---struktura s datami ktore sa nam tam objavia iba na poziadanie
|
||||
TExtraRequestData extraInfo;
|
||||
}TKobukiData;
|
||||
|
||||
|
||||
typedef long(*src_callback_kobuki_data) (void *user_data, TKobukiData &Kobuki_data);
|
||||
|
||||
class CKobuki
|
||||
{
|
||||
public:
|
||||
CKobuki() {
|
||||
stopVlakno = 0;
|
||||
std::cout << "kobuki instantiated" << std::endl;
|
||||
odometry_log.open("odometry.txt");
|
||||
};
|
||||
virtual ~CKobuki() {
|
||||
stopVlakno = 1;
|
||||
close(HCom);
|
||||
pthread_cancel(threadHandle);
|
||||
odometry_log.close();
|
||||
};
|
||||
|
||||
void enableCommands(bool commands) {
|
||||
enabledCommands = commands;
|
||||
};
|
||||
|
||||
|
||||
long loop(void *user_data, TKobukiData &Kobuki_data);
|
||||
|
||||
void startCommunication(char *portname,bool CommandsEnabled,void *userDataL);
|
||||
int measure(); //vlaknova funkcia, ma v sebe nekonecne vlakno a vycitava udaje
|
||||
void setLed(int led1 = 0, int led2 = 0); //led1 zelena/cervena 2/1, //led2 zelena/cervena 2/1
|
||||
void setTranslationSpeed(int mmpersec);
|
||||
void setRotationSpeed(double radpersec);
|
||||
void setArcSpeed(int mmpersec,int radius);
|
||||
void setSound(int noteinHz, int duration);
|
||||
void setPower(int value);
|
||||
|
||||
// control functions
|
||||
void goStraight(long double distance);
|
||||
void doRotation(long double th);
|
||||
void goToXy(long double xx, long double yy);
|
||||
std::ofstream odometry_log;
|
||||
TKobukiData data;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
private:
|
||||
int HCom;
|
||||
pthread_t threadHandle; // handle na vlakno
|
||||
int threadID; // id vlakna
|
||||
int stopVlakno;
|
||||
src_callback_kobuki_data callbackFunction;
|
||||
void *userData;
|
||||
bool enabledCommands;
|
||||
int parseKobukiMessage(TKobukiData &output, unsigned char *data );
|
||||
int connect(char *portname);
|
||||
unsigned char *readKobukiMessage();
|
||||
int checkChecksum(unsigned char *data);
|
||||
|
||||
//--spustenie merania v novom vlakne (vycitavanie bezi v novom vlakne. treba ho stopnut ak chceme poslat request)
|
||||
static void * KobukiProcess(void *param)
|
||||
{
|
||||
//std::cout << "Nase vlakno Kobuki process nastartovalo" << std::endl;
|
||||
CKobuki *hoku = (CKobuki*)param;
|
||||
int vystup = hoku->measure();
|
||||
|
||||
return param;
|
||||
}
|
||||
|
||||
|
||||
// internal variables for robot control
|
||||
int prevLeftEncoder, prevRightEncoder; // [ticks]
|
||||
uint16_t prevTimestamp;// [ms]
|
||||
long totalLeft, totalRight = 0;
|
||||
int directionL = 0; // 1 = forward, 0 = undefined, -1 = backwards
|
||||
int directionR = 0;
|
||||
int iterationCount = 0;
|
||||
long double tickToMeter = 0.000085292090497737556558; // [m/tick]
|
||||
//
|
||||
long double x = 0; // [m]
|
||||
long double y = 0;
|
||||
//
|
||||
long double theta = 0; // [rad]
|
||||
long double b = 0.23; // wheelbase distance in meters, from kobuki manual https://yujinrobot.github.io/kobuki/doxygen/enAppendixProtocolSpecification.html
|
||||
//
|
||||
long double prevGyroTheta = 0;
|
||||
long double gyroTheta = 0; // [rad]
|
||||
|
||||
// utilities
|
||||
long double gyroToRad(signed short GyroAngle);
|
||||
|
||||
// plot p;
|
||||
std::vector<float> vectorX;
|
||||
std::vector<float> vectorY;
|
||||
std::vector<float> vectorGyroTheta;
|
||||
|
||||
|
||||
double displacement = 0;
|
||||
double integratedGyroTheta = 0;
|
||||
double gx = 0;
|
||||
double gy = 0;
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
71
src/C++/Driver/src/graph.h
Normal file
71
src/C++/Driver/src/graph.h
Normal file
@@ -0,0 +1,71 @@
|
||||
#ifndef GRAPH1010
|
||||
#define GRAPH1010
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <vector>
|
||||
|
||||
using namespace std;
|
||||
#define GRAPH_ENABLED true
|
||||
|
||||
class plot {
|
||||
public:
|
||||
FILE *gp;
|
||||
bool enabled,persist;
|
||||
plot(bool _persist=false,bool _enabled=GRAPH_ENABLED) {
|
||||
enabled=_enabled;
|
||||
persist=_persist;
|
||||
if (enabled) {
|
||||
if(persist)
|
||||
gp=popen("gnuplot -persist","w");
|
||||
else
|
||||
gp=popen("gnuplot","w");
|
||||
}
|
||||
}
|
||||
|
||||
void plot_data(vector<float> x,const char* style="points",const char* title="Data") {
|
||||
if(!enabled)
|
||||
return;
|
||||
fprintf(gp,"set title '%s' \n",title);
|
||||
fprintf(gp,"plot '-' w %s \n",style);
|
||||
for(int k=0;k<x.size();k++) {
|
||||
fprintf(gp,"%f\n",x[k]);
|
||||
}
|
||||
fprintf(gp,"e\n");
|
||||
fflush(gp);
|
||||
}
|
||||
|
||||
void plot_data(vector<float> x,vector<float> y,const char* style="points",const char* title="Data") {
|
||||
if(!enabled)
|
||||
return;
|
||||
fprintf(gp,"set title '%s' \n",title);
|
||||
fprintf(gp,"plot '-' w %s \n",style);
|
||||
for(int k=0;k<x.size();k++) {
|
||||
fprintf(gp,"%f %f \n",x[k],y[k]);
|
||||
}
|
||||
fprintf(gp,"e\n");
|
||||
fflush(gp);
|
||||
}
|
||||
|
||||
~plot() {
|
||||
if(enabled)
|
||||
pclose(gp);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
/*
|
||||
int main(int argc,char **argv) {
|
||||
plot p;
|
||||
for(int a=0;a<100;a++) {
|
||||
vector<float> x,y;
|
||||
for(int k=a;k<a+200;k++) {
|
||||
x.push_back(k);
|
||||
y.push_back(k*k);
|
||||
}
|
||||
p.plot_data(x,y);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
*/
|
||||
|
||||
#endif
|
||||
63
src/C++/Driver/src/main.cpp
Normal file
63
src/C++/Driver/src/main.cpp
Normal file
@@ -0,0 +1,63 @@
|
||||
#include "CKobuki.h"
|
||||
#include <iostream>
|
||||
#include <cmath>
|
||||
#include <thread>
|
||||
#include "graph.h"
|
||||
|
||||
using namespace std;
|
||||
CKobuki robot;
|
||||
int movement();
|
||||
|
||||
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
|
||||
}
|
||||
|
||||
int checkCenterCliff()
|
||||
{
|
||||
while (true)
|
||||
{
|
||||
std::cout << "cliffsensordata:" << robot.data.CliffSensorCenter << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
int movement()
|
||||
{
|
||||
int text;
|
||||
while (true)
|
||||
{
|
||||
cin >> text;
|
||||
|
||||
if (text == 1)
|
||||
{
|
||||
robot.goStraight(10);
|
||||
}
|
||||
else if (text == 2)
|
||||
{
|
||||
// 1 is full circle
|
||||
robot.doRotation(0.25);
|
||||
}
|
||||
else if (text == 3)
|
||||
{
|
||||
// Add your code here for text == 3
|
||||
}
|
||||
else
|
||||
{
|
||||
try
|
||||
{
|
||||
robot.doRotation(text);
|
||||
throw "NaN";
|
||||
}
|
||||
catch (const char *msg)
|
||||
{
|
||||
cerr << msg << endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
BIN
src/C++/Socket/a.out
Executable file
BIN
src/C++/Socket/a.out
Executable file
Binary file not shown.
53
src/C++/Socket/main.cpp
Normal file
53
src/C++/Socket/main.cpp
Normal file
@@ -0,0 +1,53 @@
|
||||
#include <iostream>
|
||||
#include <boost/asio.hpp>
|
||||
#include <string>
|
||||
|
||||
using boost::asio::ip::tcp;
|
||||
|
||||
int main() {
|
||||
try {
|
||||
// Create an io_context object
|
||||
boost::asio::io_context io_context;
|
||||
|
||||
// Resolve the server address and port
|
||||
tcp::resolver resolver(io_context);
|
||||
tcp::resolver::results_type endpoints = resolver.resolve("127.0.0.1", "4024");
|
||||
|
||||
// Create and connect the socket
|
||||
tcp::socket socket(io_context);
|
||||
boost::asio::connect(socket, endpoints);
|
||||
|
||||
std::cout << "Connected to the server." << std::endl;
|
||||
|
||||
// Receive initial message from the server
|
||||
boost::asio::streambuf buffer;
|
||||
boost::asio::read_until(socket, buffer, "\n");
|
||||
std::istream is(&buffer);
|
||||
std::string initial_message;
|
||||
std::getline(is, initial_message);
|
||||
std::cout << "Initial message from server: " << initial_message << std::endl;
|
||||
|
||||
// Send and receive messages
|
||||
while (true) {
|
||||
// Send a message to the server
|
||||
std::string message;
|
||||
std::cout << "Enter message: ";
|
||||
std::getline(std::cin, message);
|
||||
message += "\n"; // Add newline to mark the end of the message
|
||||
boost::asio::write(socket, boost::asio::buffer(message));
|
||||
|
||||
// Receive a response from the server
|
||||
boost::asio::streambuf response_buffer;
|
||||
boost::asio::read_until(socket, response_buffer, "\n");
|
||||
std::istream response_stream(&response_buffer);
|
||||
std::string reply;
|
||||
std::getline(response_stream, reply);
|
||||
std::cout << "Reply from server: " << reply << std::endl;
|
||||
}
|
||||
|
||||
} catch (std::exception& e) {
|
||||
std::cerr << "Exception: " << e.what() << std::endl;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -1,50 +0,0 @@
|
||||
//before running set right baudrate
|
||||
//sudo stty -F /dev/ttyUSB0 speed 115200 cs8 -cstopb -parenb
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
#include <cstring>
|
||||
#include <vector>
|
||||
|
||||
int main() {
|
||||
char byte;
|
||||
bool foundAA = false;
|
||||
std::vector<char> buffer;
|
||||
std::ifstream f("/dev/ttyUSB0", std::ios::binary);
|
||||
if (!f.is_open()) {
|
||||
std::cerr << "Failed to open the file" << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
while (f.read(&byte, 1)) {
|
||||
unsigned char ubyte = static_cast<unsigned char>(byte);
|
||||
buffer.push_back(ubyte);
|
||||
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
|
||||
buffer.clear();
|
||||
foundAA = false; // Reset the state
|
||||
} else {
|
||||
foundAA = false; // Reset the state if the next byte is not 0x55
|
||||
}
|
||||
} else if (ubyte == 0b10101010) { // 0xAA
|
||||
foundAA = true;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
f.close();
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
//Tomorrow
|
||||
//Try to use vectors and store every byte in its own array object by dividing the buffer into 8 bits
|
||||
|
||||
|
||||
|
||||
|
||||
//Tomorrow
|
||||
//Try to use vectors and store every byte in its own array object by dividing the buffer into 8 bits
|
||||
BIN
src/C++/main.o
BIN
src/C++/main.o
Binary file not shown.
@@ -1,18 +0,0 @@
|
||||
#clean this up for seperate build folder
|
||||
CPPFLAGS = -Wall -I./include
|
||||
CC=g++
|
||||
|
||||
#target : main.o
|
||||
# g++ * -Wall
|
||||
progr: main.o
|
||||
$(CC) $^ $(CPPFLAGS) -o progr #S^ stands for all dependencies
|
||||
@echo "ready"
|
||||
|
||||
main.o: main.cpp #may be omitted (implicit rule)
|
||||
$(CC) $(CPPFLAGS) -c -o main.o main.cpp
|
||||
|
||||
clean:
|
||||
rm progr
|
||||
rm *.o
|
||||
|
||||
|
||||
BIN
src/C++/progr
BIN
src/C++/progr
Binary file not shown.
20
src/Python/socket/socketServer.py
Normal file
20
src/Python/socket/socketServer.py
Normal file
@@ -0,0 +1,20 @@
|
||||
import socket
|
||||
|
||||
HOST = "127.0.0.1" # Listen on all available interfaces
|
||||
PORT = 4024 # Port to listen on (non-privileged ports are > 1023)
|
||||
|
||||
with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
|
||||
s.bind((HOST, PORT))
|
||||
s.listen()
|
||||
print(f"Server listening on {HOST}:{PORT}")
|
||||
conn, addr = s.accept()
|
||||
with conn:
|
||||
print(f"Connected by {addr}")
|
||||
conn.sendall(b"hallo\n")
|
||||
while True:
|
||||
data = conn.recv(2048)
|
||||
if data:
|
||||
print("Received:", repr(data))
|
||||
conn.sendall(b"message received\n")
|
||||
if not data:
|
||||
break
|
||||
Reference in New Issue
Block a user