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 03:45:00 +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

8
.gitignore vendored
View File

@@ -23,3 +23,11 @@ src/C++/MQTT/CMakeFiles
src/C++/MQTT/Makefile
src/C++/MQTT/CMakeCache.txt
src/C++/MQTT/cmake_install.cmake
src/Python/flask/web/_pycache_
venv
build/
log
CMakeFiles/
Makefile
CMakeCache.txt
cmake_install.cmake

2
docs/Ishak/feedback.md Normal file
View File

@@ -0,0 +1,2 @@
# Feedback expert review

49
docs/Ishak/smartleerdoel.md Normal file
View File

@@ -0,0 +1,49 @@
# Smart leerdoelen
Na de retrospective die ik heb gedaan, heb ik besloten om de volgende smart leerdoelen te stellen:
1 **Ik wil in de volgende blok meer gaan opletten op mijn teamgenoten om zo goed te weten waar iedereen mee bezig is.**
**specifiek:**
ik heb in onze groep de taak als voorzitter alleen merk ik op dit moment dat ik niet goed weet waar iedereen mee bezig is. Dat is niet handig omdat ik dan als voorzitter niet goed kan inschatten of iedereen op schema ligt.
**meetbaar:**
Ik wil dat ik aan het einde van de blok meer contact heb gehad met mijn teamgenoten en dat ik weet waar iedereen mee bezig is. Dit zorgt ervoor dat ik beter kan inschatten of iedereen op schema ligt en of ik moet ingrijpen.
**acceptabel:**
Dit is acceptabel omdat ik als voorzitter de taak heb om te zorgen dat iedereen op schema ligt en dat iedereen weet wat hij/zij moet doen. Zonder deze informatie kan ik niet goed mijn taak uitvoeren waardoor de teamgenoten niet goed kunnen werken.
**realistisch:**
Dit is realistisch omdat ik als voorzitter de taak heb om te zorgen dat iedereen op schema ligt en dat iedereen weet wat hij/zij moet doen.
**tijdgebonden:**
Ik wil dit doel behalen aan het einde van de blok. Dit is een realistische tijd omdat ik dan genoeg tijd heb om dit doel te behalen. Ik kan mij later ook nog tijdens de opleiding verbeteren op dit punt.
---
2 **Ik wil in de volgende blok meer gaan opletten op mijn eigen documentatie.**
**specifiek:**
In tegen stelling tot vorig jaar hou ik nu mijn documentatie niet goed bij. Dit is niet handig omdat ik dan niet goed kan zien wat ik daadwerkelijk heb gedaan en wat ik allemaal heb geleerd. Ook is het fijn dat ik dan kan terug kijken op mijn werk als ik het later nodig heb. Ik wil daarom minimaal 2 keer per week mijn documentatie bijwerken.
**meetbaar:**
Ik wil dat ik aan het einde van de blok mijn documentatie goed heb bijgehouden. Dit zorgt ervoor dat ik kan zien wat ik heb gedaan en wat ik allemaal heb geleerd. Ook kan ik dan terug kijken op mijn werk als ik het later nodig heb.
**acceptabel:**
Dit is acceptabel omdat ik dan toekomst gericht kan werken. Ik kan dan terug kijken op mijn werk als ik het later nodig heb. Ook kan ik dan zien wat ik heb gedaan en wat ik allemaal heb geleerd.
**realistisch:**
Dit is realistisch omdat ik dit vorig jaar ook heb gedaan. Ik weet hoe ik mijn documentatie moet bijhouden en wat ik allemaal moet opschrijven. Mijn eis van 2 keer per week is ook realistisch omdat ik dan genoeg tijd heb om mijn documentatie bij te werken.
**tijdgebonden:**
Ik wil dit doel behalen aan het einde van de blok. Dit is een realistische tijd omdat ik dan genoeg tijd heb om mijn documentatie bij te werken.

BIN
docs/Ishak/smartleerdoel.pdf Normal file
View File

Binary file not shown.

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