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

Merge branch 'test_script_movement'

Browse Source
This commit is contained in:
Sam Hos
2024-10-31 21:51:04 +01:00
parent 2919e20970 0f8d68f391
commit a46900dfd2
27 changed files with 1187 additions and 864 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,12 +1,23 @@
cmake_minimum_required(VERSION 3.9)
project(kobuki_control)
set(CMAKE_CXX_STANDARD 23)
# Find the Paho MQTT C++ library (static)
find_library(PAHO_MQTTPP_LIBRARY paho-mqttpp3 PATHS /usr/local/lib)
find_library(PAHO_MQTT_LIBRARY paho-mqtt3a PATHS /usr/local/lib)
include_directories(/usr/local/include)
set(SOURCE_FILES
src/KobukiParser.cpp
src/KobukiParser.h
src/CKobuki.cpp
src/CKobuki.h
src/main.cpp)
src/KobukiDriver/KobukiParser.cpp
src/KobukiDriver/KobukiParser.h
src/KobukiDriver/CKobuki.cpp
src/KobukiDriver/CKobuki.h
src/MQTT/MqttClient.cpp
src/MQTT/MqttClient.h
src/main.cpp)
add_executable(kobuki_control ${SOURCE_FILES})
#target_link_libraries(kobuki_control )
# Link the static libraries
target_link_libraries(kobuki_control ${PAHO_MQTTPP_LIBRARY} ${PAHO_MQTT_LIBRARY} pthread)

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

@@ -1,683 +0,0 @@
#include "CKobuki.h"
#include "errno.h"
#include "termios.h"
#include <cstddef>
#include <iostream>
#include <thread>
// 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;
}
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);
if (pthread_result != 0) {
std::cerr << "Error creating thread: " << pthread_result << std::endl;
}
}
int CKobuki::measure()
{
while (stopVlakno == 0)
{
unsigned char *message = readKobukiMessage();
if (message == NULL)
{
// printf("vratil null message\n");
continue;
}
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, parser.data);
}
free(message);
}
return 0;
}
long double CKobuki::gyroToRad(signed short GyroAngle)
{
long double rad;
if (GyroAngle < 0)
{
rad = GyroAngle + 360;
}
else
{
rad = GyroAngle;
}
return (long double)rad * PI / 180.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;
//vector for data plotting
vectorX.push_back(gx);
vectorY.push_back(gy);
vectorGyroTheta.push_back(gyroTheta);
// if (counter % 100 == 0) {
// p.plot_data(vectorY, vectorX);
// }
counter++;
return 0;
}
// tells the kobuki to go a few meters forward or backward, the sign decides
// the function compensates for walking straight with the controller, internally it uses setArcSpeed and
// uses encoder data as feedback
void CKobuki::goStraight(long double distance){
long double u_translation = 0; // controlled magnitude, speed of the robot in motion
long double w_translation = distance; // requested value
// controller parameters
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; // controlled magnitude
long double w_rotation = 0;
long double Kp_rotation = 57;
long double e_rotation = 0;
x = 0;
y = 0;
theta = 0;
long i = 5;
//send command and hold until robot reaches point
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;
}
//send command to robot
this->setArcSpeed(u_translation, u_rotation);
// 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; // controlled variable, angular speed of the robot during movement
long double w = th; // desired value in radians
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 << "Rotation done" << std::endl;
this->setRotationSpeed(0);
usleep(25*1000);
}
// combines navigation to a coordinate and rotation by an angle, performs movement to
// the selected coordinate in the robot's coordinate system
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;
}
/// @brief Makes the Kobuki go forward
/// @param speedvalue speed of robot in mm/s
/// @param distance distance in meters
void CKobuki::forward(int speedvalue) {
// Use the goStraight logic to determine the speed and distance
// Calculate the actual speed and radius values based on the conversion table
int actual_speed = speedvalue;
int actual_radius = 0; // Pure translation (straight line)
unsigned char message[11] = {
0xaa, // Start byte 1
0x55, // Start byte 2
0x08, // Payload length (the first 2 bytes dont count)
0x01, // payload type (0x01 = control command)
0x04, // Control byte or additional identifier
actual_speed % 256, // Lower byte of speed value
actual_speed >> 8, // Upper byte of speed value
0x00, // Placeholder for radius
0x00, // Placeholder for radius
0x00 // Placeholder for checksum
};
// Calculate checksum
message[10] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^
message[7] ^ message[8] ^ message[9];
// Send the message
uint32_t pocet;
pocet = write(HCom, &message, 11);
pocet = write(HCom, &message, 11);
}
/// @brief Makes the kobuki rotate
/// @param degrees Rotation in degrees
void CKobuki::Rotate(int degrees) {
// convert raidans to degrees
float radians = degrees * PI / 180.0;
// Calculate the rotation speed in radians per second
double radpersec = 1;
// calculator rotation time and give absolute value
float rotation_time = std::abs(radians / radpersec);
// Use original function to set the rotation speed in mm/s
setRotationSpeed(radians);
// Sleep for the calculated rotation time
std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(rotation_time * 1000)));
// Stop the robot after the rotation
setRotationSpeed(0);
}
void CKobuki::robotSafety() {
while (true) {
if (parser.data.BumperCenter || parser.data.BumperLeft || parser.data.BumperRight ||
parser.data.CliffLeft || parser.data.CliffCenter || parser.data.CliffRight) {
std::cout << "Safety condition triggered!" << std::endl; // Debug print
forward(-100); // reverse the robot
}
}
}
void CKobuki::sendNullMessage(){
unsigned char message[11] = {
0xaa, // Start byte 1
0x55, // Start byte 2
0x08, // Payload length (the first 2 bytes dont count)
0x01, // payload type (0x01 = control command)
0x04, // Control byte or additional identifier
0x00, // Lower byte of speed value
0x00, // Upper byte of speed value
0x00, // Placeholder for radius
0x00, // Placeholder for radius
0x00 // Placeholder for checksum
};
message[10] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^
message[7] ^ message[8] ^ message[9];
// Send the message
uint32_t pocet;
pocet = write(HCom, &message, 11);
}

757
src/C++/Driver/src/KobukiDriver/CKobuki.cpp Executable file
View File

@@ -0,0 +1,757 @@
#include "CKobuki.h"
#include "errno.h"
#include "termios.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;
}
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);
if (pthread_result != 0) {
std::cerr << "Error creating thread: " << pthread_result << std::endl;
}
}
int CKobuki::measure() {
while (stopVlakno == 0) {
unsigned char *message = readKobukiMessage();
if (message == NULL) {
// printf("vratil null message\n");
continue;
}
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, 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 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 rad;
if (GyroAngle < 0) {
rad = GyroAngle + 360;
} else {
rad = GyroAngle;
}
return (long double)rad * PI / 180.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;
}
// tells the kobuki to go a few meters forward or backward, the sign decides
// the function compensates for walking straight with the controller, internally
// it uses setArcSpeed and uses encoder data as feedback
void CKobuki::goStraight(long double distance) {
long double u_translation =
0; // controlled magnitude, speed of the robot in motion
long double w_translation = distance; // requested value
// controller parameters
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; // controlled magnitude
long double w_rotation = 0;
long double Kp_rotation = 57;
long double e_rotation = 0;
x = 0;
y = 0;
theta = 0;
long i = 5;
// send command and hold until robot reaches point
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;
}
// send command to robot
this->setArcSpeed(u_translation, u_rotation);
// 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; // controlled variable, angular speed of the robot during movement
long double w = th; // desired value in radians
long double Kp = PI;
long double e = 0;
long double 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 << "Rotation done" << std::endl;
this->setRotationSpeed(0);
usleep(25 * 1000);
}
// combines navigation to a coordinate and rotation by an angle, performs
// movement to the selected coordinate in the robot's coordinate system
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;
}
void CKobuki::forward(int speedvalue, long double distance) {
// Use the goStraight logic to determine the speed and distance
// Calculate the actual speed and radius values based on the conversion table
int actual_speed = speedvalue;
int actual_radius = 0; // Pure translation (straight line)
unsigned char message[11] = {
0xaa, // Start byte 1
0x55, // Start byte 2
0x08, // Payload length (the first 2 bytes dont count)
0x01, // payload type (0x01 = control command)
0x04, // Control byte or additional identifier
actual_speed % 256, // Lower byte of speed value
actual_speed >> 8, // Upper byte of speed value
0x00, // Placeholder for radius
0x00, // Placeholder for radius
0x00 // Placeholder for checksum
};
// Calculate checksum
message[10] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^
message[7] ^ message[8] ^ message[9];
// Send the message
uint32_t pocet;
pocet = write(HCom, &message, 11);
}

0
src/C++/Driver/src/CKobuki.h → src/C++/Driver/src/KobukiDriver/CKobuki.h
View File

2
src/C++/Driver/src/KobukiParser.cpp → src/C++/Driver/src/KobukiDriver/KobukiParser.cpp
View File

@@ -4,7 +4,7 @@
int KobukiParser::parseKobukiMessage(TKobukiData &output, unsigned char *data) {
int rtrnvalue = checkChecksum(data);
if (rtrnvalue != 0) {
std::cerr << "Invalid checksum" << std::endl;
// std::cerr << "Invalid checksum" << std::endl;
return -2;
}

0
src/C++/Driver/src/KobukiParser.h → src/C++/Driver/src/KobukiDriver/KobukiParser.h
View File

0
src/C++/Driver/src/graph.h → src/C++/Driver/src/KobukiDriver/graph.h
View File

30
src/C++/Driver/src/MQTT/CMakeLists.txt Normal file
View File

@@ -0,0 +1,30 @@
cmake_minimum_required(VERSION 3.10)
set(CMAKE_CXX_STANDARD 23)
# Project name
project(mqtt_receiver)
# Find the Paho MQTT C++ library
find_library(PAHO_MQTTPP_LIBRARY paho-mqttpp3 PATHS /usr/local/lib)
find_library(PAHO_MQTT_LIBRARY paho-mqtt3a PATHS /usr/local/lib)
# Include the headers
include_directories(/usr/local/include)
# Set source files
set(SOURCE_FILES
main.cpp
MqttClient.cpp
MqttClient.h
)
# Add the executable
add_executable(mqtt_receiver ${SOURCE_FILES})
# Link the libraries
# Include directories for headers
target_include_directories(mqtt_receiver PRIVATE)
find_package(Threads REQUIRED)
target_link_libraries(mqtt_receiver Threads::Threads)

61
src/C++/Driver/src/MQTT/MqttClient.cpp Normal file
View File

@@ -0,0 +1,61 @@
#include "MqttClient.h"
MqttClient::MqttClient(const std::string& address, const std::string& clientId, const std::string& username, const std::string& password)
: client_(address, clientId), username_(username), password_(password), callback_(*this) {
client_.set_callback(callback_);
connOpts_.set_clean_session(true);
connOpts_.set_mqtt_version(MQTTVERSION_3_1_1); // For MQTT 3.1.1
if (!username_.empty() && !password_.empty()) {
connOpts_.set_user_name(username_);
connOpts_.set_password(password_);
}
}
void MqttClient::connect() {
try {
std::cout << "Connecting to broker..." << std::endl;
client_.connect(connOpts_)->wait();
std::cout << "Connected!" << std::endl;
} catch (const mqtt::exception& exc) {
std::cerr << "Error: " << exc.what() << std::endl;
throw;
}
}
void MqttClient::subscribe(const std::string& topic, int qos) {
try {
std::cout << "Subscribing to topic: " << topic << std::endl;
client_.subscribe(topic, qos)->wait();
} catch (const mqtt::exception& exc) {
std::cerr << "Error: " << exc.what() << std::endl;
throw;
}
}
/// @brief Only needed when program doesnt keep itself alive
void MqttClient::run() {
// Keep the client running to receive messages
while (true) {
std::this_thread::sleep_for(std::chrono::seconds(1)); // Wait to reduce CPU usage
}
}
void MqttClient::Callback::message_arrived(mqtt::const_message_ptr msg) {
std::lock_guard<std::mutex> lock(client_.messageMutex_);
client_.lastMessage_ = msg->to_string();
}
void MqttClient::Callback::connection_lost(const std::string& cause) {
std::cerr << "Connection lost. Cause: " << cause << std::endl;
}
void MqttClient::Callback::delivery_complete(mqtt::delivery_token_ptr token) {
std::cout << "Message delivered!" << std::endl;
}
/// @brief Get the last message received from the MQTT broker
/// @return The last message received in a string
std::string MqttClient::getLastMessage() {
std::lock_guard<std::mutex> lock(messageMutex_);
return lastMessage_;
}

38
src/C++/Driver/src/MQTT/MqttClient.h Normal file
View File

@@ -0,0 +1,38 @@
#ifndef MQTTCLIENT_H
#define MQTTCLIENT_H
#include <iostream>
#include <thread>
#include <mutex>
#include <mqtt/async_client.h>
class MqttClient {
public:
MqttClient(const std::string& address, const std::string& clientId, const std::string& username = "", const std::string& password = "");
void connect();
void subscribe(const std::string& topic, int qos = 1);
void run();
std::string getLastMessage();
private:
class Callback : public virtual mqtt::callback {
public:
Callback(MqttClient& client) : client_(client) {}
void message_arrived(mqtt::const_message_ptr msg) override;
void connection_lost(const std::string& cause) override;
void delivery_complete(mqtt::delivery_token_ptr token) override;
private:
MqttClient& client_;
};
mqtt::async_client client_;
mqtt::connect_options connOpts_;
Callback callback_;
std::string username_;
std::string password_;
std::string lastMessage_;
std::mutex messageMutex_;
};
#endif // MQTTCLIENT_H

10
src/C++/Driver/src/MQTT/example.cpp Normal file
View File

@@ -0,0 +1,10 @@
#include "MqttClient.h"
int main(){
MqttClient client("mqtt://localhost:1883", "raspberry_pi_client", "ishak", "kobuki");
client.connect();
client.subscribe("home/commands");
client.run();
return 0;
}

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

@@ -1,35 +1,45 @@
#include "CKobuki.h"
#include <iostream>
#include <cmath>
#include <thread>
#include "graph.h"
#include "KobukiDriver/graph.h"
#include "MQTT/MqttClient.h"
#include "KobukiDriver/CKobuki.h"
using namespace std;
CKobuki robot;
int movement();
int checkCenterCliff();
void logToFile();
std::string readMQTT();
MqttClient client("mqtt://localhost:1883", "KobukiRPI", "ishak", "kobuki");
int main()
{
unsigned char *null_ptr(0);
robot.startCommunication("/dev/ttyUSB0", true, null_ptr);
std::thread safety([&robot]()
{ robot.robotSafety(); }); // use a lambda function to call the member function
safety.detach();
void setup(){
unsigned char *null_ptr(0);
robot.startCommunication("/dev/ttyUSB0", true, null_ptr);
client.connect();
client.subscribe("home/commands");
thread movementThread(movement);
movementThread.join(); // so the program doesnt quit
return 0;
}
int checkCenterCliff()
int main(){
setup();
while(true){
readMQTT();
}
client.run();
return 0;
}
std::string readMQTT()
{
while (true)
{
std::cout << robot.parser.data.CliffSensorRight << endl;
}
std::string message = client.getLastMessage();
if (!message.empty()) {
std::cout << "MQTT Message: " << message << std::endl;
}
// Add a small delay to avoid busy-waiting
std::this_thread::sleep_for(std::chrono::milliseconds(100));
return message;
}
int movement()

100
src/C++/Driver/src/test.cpp
View File

@@ -1,67 +1,61 @@
#include "CKobuki.h"
#include <iostream>
#include <cmath>
#include <thread>
#include "graph.h"
#include <cmath>
#include <iostream>
#include <thread>
using namespace std;
// Globale instantie van de CKobuki-klasse
CKobuki robot;
// Functieprototypes
int command();
int checkCenterCliff();
int checkWheelDrop();
const int forward = 1;
const int ROTATE = 2;
int main() {
// Start communicatie met de robot
unsigned char *null_ptr(0);
robot.startCommunication("/dev/ttyUSB0", true, null_ptr);
usleep(1 * 1000 * 1000);
// Start een nieuwe thread voor de command-functie
thread mv(command);
usleep(30 * 1000 * 1000);
mv.join(); // Wacht tot de command-thread klaar is
int main()
{
unsigned char *null_ptr(0);
robot.startCommunication("/dev/ttyUSB0", true, null_ptr);
usleep(1 * 1000 * 1000);
thread mv(command);
usleep(30 * 1000 * 1000);
mv.join(); //only exit once thread one is done running
}
// int checkCenterCliff()
// {
// while (true)
// {
// std::cout << "cliffsensordata:" << robot.parser.data.CliffSensorCenter << std::endl;
// }
// }
// Functie om commando's van de gebruiker te verwerken
int command() {
int input;
// int checkWheelDrop(){
// while (true)
// {
// std::cout << "wheeldropdata:" << robot.parser.data.WheelDropLeft << std::endl;
// }
// }
while (true) {
// Vraag de gebruiker om een commando
std::cout << "choose between forward = 1 or rotate = 2" << endl;
std::cout << "What must the robot do?";
std::cin >> input;
int command(){
cout << "choose between forward and rotate" << endl;
cout << "What must the robot do?";
cin >> input;
// Verwerk de invoer van de gebruiker
switch (input) {
case 1: {
int distance;
std::cout << "Enter distance to move forward: ";
std::cin >> distance;
robot.goStraight(distance); // Beweeg de robot naar voren
} break;
switch(input){
case forward:{
int distance;
std::cout >> "Enter distance to move forward: ";
std::cin >> distance;
robot.goStraight(distance);
}
case ROTATE:{
int angle;
std::cout >> "Enter angle to rotate: ";
std::cin >> angle;
robot.doRotation(angle);
}
robot.goStraight(-1);
break;
default:
cout << "Invalid input" << endl;
break;
}
}
case 2: {
int angle;
std::cout << "Enter angle to rotate in degrees: ";
std::cin >> angle;
robot.doRotation(angle); // Draai de robot
} break;
default:
cout << "Invalid input" << endl; // Ongeldige invoer
break;
}
}
}

15
src/C++/MQTT/CMakeLists.txt Normal file
View File

@@ -0,0 +1,15 @@
cmake_minimum_required(VERSION 3.10)
set(CMAKE_CXX_STANDARD 23)
# Find the Paho MQTT C++ library
find_library(PAHO_MQTTPP_LIBRARY paho-mqttpp3 PATHS /usr/local/lib)
find_library(PAHO_MQTT_LIBRARY paho-mqtt3a PATHS /usr/local/lib)
# Include the headers
include_directories(/usr/local/include)
# Add the executable
add_executable(my_program main.cpp)
# Link the libraries
target_link_libraries(my_program ${PAHO_MQTTPP_LIBRARY} ${PAHO_MQTT_LIBRARY})

56
src/C++/MQTT/main.cpp Normal file
View File

@@ -0,0 +1,56 @@
#include <iostream>
#include <mqtt/async_client.h>
#include <thread> // Voor std::this_thread::sleep_for
#include <chrono> // Voor std::chrono::seconds
const std::string ADDRESS("mqtt://localhost:1883"); // Aanpassen indien nodig
const std::string CLIENT_ID("raspberry_pi_client");
const std::string TOPIC("home/commands");
class callback : public virtual mqtt::callback {
void message_arrived(mqtt::const_message_ptr msg) override {
std::cout << "Ontvangen bericht: '" << msg->get_topic()
<< "' : " << msg->to_string() << std::endl;
// Doe iets met het bericht, bijvoorbeeld een GP.IO-activering
}
void connection_lost(const std::string& cause) override {
std::cerr << "Verbinding verloren. Oorzaak: " << cause << std::endl;
}
void delivery_complete(mqtt::delivery_token_ptr token) override {
std::cout << "Bericht afgeleverd!" << std::endl;
}
};
int main() {
mqtt::async_client client(ADDRESS, CLIENT_ID);
callback cb;
client.set_callback(cb);
mqtt::connect_options connOpts;
connOpts.set_clean_session(true);
connOpts.set_user_name("ishak");
connOpts.set_password("kobuki");
connOpts.set_mqtt_version(MQTTVERSION_3_1_1); // Voor MQTT 3.1.1
try {
std::cout << "Verbinden met broker..." << std::endl;
client.connect(connOpts)->wait();
std::cout << "Verbonden!" << std::endl;
std::cout << "Abonneren op topic: " << TOPIC << std::endl;
client.subscribe(TOPIC, 1)->wait();
// Houd de client draaiende om berichten te blijven ontvangen
while (true) {
std::this_thread::sleep_for(std::chrono::seconds(1)); // Wacht om CPU-gebruik te verminderen
}
} catch (const mqtt::exception &exc) {
std::cerr << "Fout: " << exc.what() << std::endl;
return 1;
}
return 0;
}

BIN
src/C++/Socket/a.out
View File

Binary file not shown.

53
src/C++/Socket/main.cpp
View File

@@ -1,53 +0,0 @@
#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;
}

BIN
src/Python/flask/web/__pycache__/app.cpython-311.pyc Normal file
View File

Binary file not shown.

33
src/Python/flask/web/app.py
View File

@@ -1,17 +1,32 @@
from flask import Flask, render_template
from flask import Flask, request, render_template, jsonify
import paho.mqtt.client as mqtt
app = Flask(__name__)
@app.route('/')
# Create an MQTT client instance
mqtt_client = mqtt.Client()
mqtt_client.username_pw_set("ishak", "kobuki")
mqtt_client.connect("localhost", 1883, 60)
mqtt_client.loop_start()
@app.route('/', methods=["POST"])
def index():
return render_template('index.html')
@app.route('/control', methods=['POST'])
def control():
return("hello")
@app.route('/move', methods=['POST'])
def move():
# Get the direction from the form data
direction = request.form['direction']
# Publish the direction to the MQTT topic "home/commands"
result = mqtt_client.publish("home/commands", direction)
# Check if the publish was successful
if result.rc == mqtt.MQTT_ERR_SUCCESS:
return jsonify({"message": "Bericht succesvol gepubliceerd"}), 200
else:
return jsonify({"message": "Fout bij het publiceren van bericht"}), 500
# Run the Flask application in debug mode
if __name__ == '__main__':
app.run(debug=True)
app.run(debug=True)

14
src/Python/flask/web/static/script.js Normal file
View File

@@ -0,0 +1,14 @@
window.onload = function () {
const form = document.getElementById("form");
form.onclick = function (event) {
event.preventDefault();
fetch("/move", {
method: "GET",
})
.then((response) => {})
.then(() => {
fetch
});
};
};
9

4
src/Python/flask/web/static/style.css
View File

@@ -10,7 +10,7 @@ body {
.navbar {
display: flex;
justify-content: space-between;
max-width: 70rem;
max-width: 80%;
background-color: #fff;
border: 1px solid #f0f0f0;
border-radius: 50px;
@@ -56,8 +56,6 @@ body {
position: relative;
width: 150px;
height: 150px;
margin-left: auto;
margin-right: auto;
}
.btn {

45
src/Python/flask/web/templates/index.html
View File

@@ -1,19 +1,30 @@
{%extends 'base.html'%}
{%block head%}
{%extends 'base.html'%} {%block head%} {%endblock%} {%block content%}
<!DOCTYPE html>
<html lang="en">
<head>
<meta charset="UTF-8" />
<meta name="viewport" content="width=device-width, initial-scale=1.0" />
<title>Kobuki</title>
</head>
<body>
<div class="container">
<div class="image-section">
<img src="kobuki.jpg" alt="Kobuki Robot" id="robot-image" />
</div>
<div class="button-section">
<form id = "form" action="/" method="post">
<button class="btn" name="direction" value="left">←</button>
<button class="btn" name="direction" value="up">↑</button>
<button class="btn" name="direction" value="right">→</button>
<button class="btn" name="direction" value="down">↓</button>
</form>
</div>
</div>
<div class="container">
<h1>Sensor Data</h1>
</div>
<script href="../static/script.js"></script>
</body>
</html>
{%endblock%}
{%block content%}
<div class="container">
<div class="image-section">
<img src="kobuki.jpg" alt="Kobuki Robot" id="robot-image">
</div>
<div class="button-section">
<button class="btn">1</button>
<button class="btn">2</button>
<button class="btn">3</button>
<button class="btn">4</button>
</div>
</div>
{%endblock%}

20
src/Python/socket/socketServer.py
View File

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