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

fixed spacing

Browse Source
This commit is contained in:
Mees Roelofsz
2024-10-02 12:38:19 +02:00
parent 163e7d4b7c
commit d4ba49eb5c
1 changed files with 693 additions and 613 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -4,17 +4,18 @@
#include <cstddef> #include <cstddef>
#include <iostream> #include <iostream>
// plot p; // plot p;
static std::vector<float> vectorX; static std::vector<float> vectorX;
static std::vector<float> vectorY; static std::vector<float> vectorY;
static std::vector<float> vectorGyroTheta; static std::vector<float> vectorGyroTheta;
// obsluha tty pod unixom // obsluha tty pod unixom
int set_interface_attribs2(int fd, int speed, int parity) { int set_interface_attribs2(int fd, int speed, int parity)
{
struct termios tty; struct termios tty;
memset(&tty, 0, sizeof tty); memset(&tty, 0, sizeof tty);
if (tcgetattr(fd, &tty) != 0) { if (tcgetattr(fd, &tty) != 0)
{
printf("error %d from tcgetattr", errno); printf("error %d from tcgetattr", errno);
return -1; return -1;
} }
@@ -42,17 +43,20 @@ int set_interface_attribs2(int fd, int speed, int parity) {
tty.c_cflag &= ~CSTOPB; tty.c_cflag &= ~CSTOPB;
tty.c_cflag &= ~CRTSCTS; tty.c_cflag &= ~CRTSCTS;
if (tcsetattr(fd, TCSANOW, &tty) != 0) { if (tcsetattr(fd, TCSANOW, &tty) != 0)
{
printf("error %d from tcsetattr", errno); printf("error %d from tcsetattr", errno);
return -1; return -1;
} }
return 0; return 0;
} }
void set_blocking2(int fd, int should_block) { void set_blocking2(int fd, int should_block)
{
struct termios tty; struct termios tty;
memset(&tty, 0, sizeof tty); memset(&tty, 0, sizeof tty);
if (tcgetattr(fd, &tty) != 0) { if (tcgetattr(fd, &tty) != 0)
{
printf("error %d from tggetattr", errno); printf("error %d from tggetattr", errno);
return; return;
} }
@@ -64,14 +68,17 @@ void set_blocking2(int fd, int should_block) {
printf("error %d setting term attributes", errno); printf("error %d setting term attributes", errno);
} }
int CKobuki::connect(char *comportT) { int CKobuki::connect(char *comportT)
{
HCom = open(comportT, O_RDWR | O_NOCTTY | O_NONBLOCK); HCom = open(comportT, O_RDWR | O_NOCTTY | O_NONBLOCK);
if (HCom == -1) { if (HCom == -1)
{
printf("Kobuki nepripojeny\n"); printf("Kobuki nepripojeny\n");
return HCom; return HCom;
}
} else { else
{
set_interface_attribs2(HCom, B115200, set_interface_attribs2(HCom, B115200,
0); // set speed to 115,200 bps, 8n1 (no parity) 0); // set speed to 115,200 bps, 8n1 (no parity)
set_blocking2(HCom, 0); // set no blocking set_blocking2(HCom, 0); // set no blocking
@@ -95,31 +102,37 @@ int CKobuki::connect(char *comportT) {
} }
} }
unsigned char *CKobuki::readKobukiMessage() { unsigned char *CKobuki::readKobukiMessage()
{
unsigned char buffer[1]; unsigned char buffer[1];
ssize_t Pocet; ssize_t Pocet;
buffer[0] = 0; buffer[0] = 0;
unsigned char *null_buffer(0); unsigned char *null_buffer(0);
// citame kym nezachytime zaciatok spravy // citame kym nezachytime zaciatok spravy
do { do
{
Pocet = read(HCom, buffer, 1); Pocet = read(HCom, buffer, 1);
} while (buffer[0] != 0xAA); } while (buffer[0] != 0xAA);
// mame zaciatok spravy (asi) // mame zaciatok spravy (asi)
if (Pocet == 1 && buffer[0] == 0xAA) { if (Pocet == 1 && buffer[0] == 0xAA)
{
// citame dalsi byte // citame dalsi byte
do { do
{
Pocet = read(HCom, buffer, 1); Pocet = read(HCom, buffer, 1);
} while (Pocet != 1); // na linuxe -1 na windowse 0 } while (Pocet != 1); // na linuxe -1 na windowse 0
// a ak je to druhy byte hlavicky // a ak je to druhy byte hlavicky
if (Pocet == 1 && buffer[0] == 0x55) { if (Pocet == 1 && buffer[0] == 0x55)
{
// precitame dlzku // precitame dlzku
Pocet = read(HCom, buffer, 1); Pocet = read(HCom, buffer, 1);
// ReadFile(hCom, buffer, 1, &Pocet, NULL); // ReadFile(hCom, buffer, 1, &Pocet, NULL);
if (Pocet == 1) { if (Pocet == 1)
{
// mame dlzku.. nastavime vektor a precitame ho cely // mame dlzku.. nastavime vektor a precitame ho cely
int readLenght = buffer[0]; int readLenght = buffer[0];
unsigned char *outputBuffer = unsigned char *outputBuffer =
@@ -127,7 +140,8 @@ unsigned char *CKobuki::readKobukiMessage() {
outputBuffer[0] = buffer[0]; outputBuffer[0] = buffer[0];
int pct = 0; int pct = 0;
do { do
{
Pocet = 0; Pocet = 0;
int readpoc = (readLenght + 1 - pct); int readpoc = (readLenght + 1 - pct);
Pocet = read(HCom, outputBuffer + 1 + pct, readpoc); Pocet = read(HCom, outputBuffer + 1 + pct, readpoc);
@@ -149,15 +163,18 @@ unsigned char *CKobuki::readKobukiMessage() {
return null_buffer; return null_buffer;
} }
int CKobuki::checkChecksum(unsigned char *data) { // najprv hlavicku int CKobuki::checkChecksum(unsigned char *data)
{ // najprv hlavicku
unsigned char chckSum = 0; unsigned char chckSum = 0;
for (int i = 0; i < data[0] + 2; i++) { for (int i = 0; i < data[0] + 2; i++)
{
chckSum ^= data[i]; chckSum ^= data[i];
} }
return chckSum; // 0 ak je vsetko v poriadku,inak nejake cislo return chckSum; // 0 ak je vsetko v poriadku,inak nejake cislo
} }
void CKobuki::setLed(int led1, int led2) { void CKobuki::setLed(int led1, int led2)
{
unsigned char message[8] = {0xaa, 0x55, 0x04, 0x0c, 0x02, 0x00, (unsigned char)((led1 + led2 * 4) % 256), 0x00}; unsigned char message[8] = {0xaa, 0x55, 0x04, 0x0c, 0x02, 0x00, (unsigned char)((led1 + led2 * 4) % 256), 0x00};
message[7] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6]; message[7] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6];
uint32_t pocet; uint32_t pocet;
@@ -166,15 +183,18 @@ void CKobuki::setLed(int led1, int led2) {
// tato funkcia nema moc sama o sebe vyznam, payload o tom, ze maju byt externe // tato funkcia nema moc sama o sebe vyznam, payload o tom, ze maju byt externe
// napajania aktivne musi byt aj tak v kazdej sprave... // napajania aktivne musi byt aj tak v kazdej sprave...
void CKobuki::setPower(int value) { void CKobuki::setPower(int value)
if (value == 1) { {
if (value == 1)
{
unsigned char message[8] = {0xaa, 0x55, 0x04, 0x0C, 0x02, 0xf0, 0x00, 0xAF}; unsigned char message[8] = {0xaa, 0x55, 0x04, 0x0C, 0x02, 0xf0, 0x00, 0xAF};
uint32_t pocet; uint32_t pocet;
pocet = write(HCom, &message, 8); pocet = write(HCom, &message, 8);
} }
} }
void CKobuki::setTranslationSpeed(int mmpersec) { void CKobuki::setTranslationSpeed(int mmpersec)
{
unsigned char message[14] = {0xaa, 0x55, 0x0A, 0x0c, 0x02, unsigned char message[14] = {0xaa, 0x55, 0x0A, 0x0c, 0x02,
0xf0, 0x00, 0x01, 0x04, mmpersec % 256, 0xf0, 0x00, 0x01, 0x04, mmpersec % 256,
mmpersec >> 8, 0x00, 0x00, 0x00}; mmpersec >> 8, 0x00, 0x00, 0x00};
@@ -186,7 +206,8 @@ void CKobuki::setTranslationSpeed(int mmpersec) {
pocet = write(HCom, &message, 14); pocet = write(HCom, &message, 14);
} }
void CKobuki::setRotationSpeed(double radpersec) { void CKobuki::setRotationSpeed(double radpersec)
{
int speedvalue = radpersec * 230.0f / 2.0f; int speedvalue = radpersec * 230.0f / 2.0f;
unsigned char message[14] = {0xaa, unsigned char message[14] = {0xaa,
0x55, 0x55,
@@ -210,8 +231,10 @@ void CKobuki::setRotationSpeed(double radpersec) {
pocet = write(HCom, &message, 14); pocet = write(HCom, &message, 14);
} }
void CKobuki::setArcSpeed(int mmpersec, int radius) { void CKobuki::setArcSpeed(int mmpersec, int radius)
if (radius == 0) { {
if (radius == 0)
{
setTranslationSpeed(mmpersec); setTranslationSpeed(mmpersec);
return; return;
} }
@@ -239,7 +262,8 @@ void CKobuki::setArcSpeed(int mmpersec, int radius) {
pocet = write(HCom, &message, 14); pocet = write(HCom, &message, 14);
} }
void CKobuki::setSound(int noteinHz, int duration) { void CKobuki::setSound(int noteinHz, int duration)
{
int notevalue = floor((double)1.0 / ((double)noteinHz * 0.00000275) + 0.5); int notevalue = floor((double)1.0 / ((double)noteinHz * 0.00000275) + 0.5);
unsigned char message[9] = {0xaa, 0x55, 0x05, unsigned char message[9] = {0xaa, 0x55, 0x05,
0x03, 0x03, notevalue % 256, 0x03, 0x03, notevalue % 256,
@@ -252,7 +276,8 @@ void CKobuki::setSound(int noteinHz, int duration) {
} }
void CKobuki::startCommunication(char *portname, bool CommandsEnabled, void CKobuki::startCommunication(char *portname, bool CommandsEnabled,
void *userDataL) { void *userDataL)
{
connect(portname); connect(portname);
enableCommands(CommandsEnabled); enableCommands(CommandsEnabled);
userData = userDataL; userData = userDataL;
@@ -262,10 +287,13 @@ void CKobuki::startCommunication(char *portname, bool CommandsEnabled,
pthread_create(&threadHandle, NULL, KobukiProcess, (void *)this); pthread_create(&threadHandle, NULL, KobukiProcess, (void *)this);
} }
int CKobuki::measure() { int CKobuki::measure()
while (stopVlakno == 0) { {
while (stopVlakno == 0)
{
unsigned char *message = readKobukiMessage(); unsigned char *message = readKobukiMessage();
if (message == NULL) { if (message == NULL)
{
// printf("vratil null message\n"); // printf("vratil null message\n");
continue; continue;
} }
@@ -273,7 +301,8 @@ int CKobuki::measure() {
// maximalne moze trvat callback funkcia 20 ms, ak by trvala viac, nestihame // maximalne moze trvat callback funkcia 20 ms, ak by trvala viac, nestihame
// citat // citat
if (ok == 0) { if (ok == 0)
{
loop(userData, data); loop(userData, data);
} }
free(message); free(message);
@@ -281,7 +310,8 @@ int CKobuki::measure() {
return 0; return 0;
} }
int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char *data) { int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char *data)
{
int rtrnvalue = checkChecksum(data); int rtrnvalue = checkChecksum(data);
// ak je zly checksum,tak kaslat na to // ak je zly checksum,tak kaslat na to
if (rtrnvalue != 0) if (rtrnvalue != 0)
@@ -289,9 +319,11 @@ int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char *data) {
int checkedValue = 1; int checkedValue = 1;
// kym neprejdeme celu dlzku // kym neprejdeme celu dlzku
while (checkedValue < data[0]) { while (checkedValue < data[0])
{
// basic data subload // basic data subload
if (data[checkedValue] == 0x01) { if (data[checkedValue] == 0x01)
{
checkedValue++; checkedValue++;
if (data[checkedValue] != 0x0F) if (data[checkedValue] != 0x0F)
return -1; return -1;
@@ -325,7 +357,9 @@ int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char *data) {
checkedValue++; checkedValue++;
output.overCurrent = data[checkedValue]; output.overCurrent = data[checkedValue];
checkedValue++; checkedValue++;
} else if (data[checkedValue] == 0x03) { }
else if (data[checkedValue] == 0x03)
{
checkedValue++; checkedValue++;
if (data[checkedValue] != 0x03) if (data[checkedValue] != 0x03)
return -3; return -3;
@@ -336,7 +370,9 @@ int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char *data) {
checkedValue++; checkedValue++;
output.IRSensorLeft = data[checkedValue]; output.IRSensorLeft = data[checkedValue];
checkedValue++; checkedValue++;
} else if (data[checkedValue] == 0x04) { }
else if (data[checkedValue] == 0x04)
{
checkedValue++; checkedValue++;
if (data[checkedValue] != 0x07) if (data[checkedValue] != 0x07)
return -4; return -4;
@@ -345,7 +381,9 @@ int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char *data) {
checkedValue += 2; checkedValue += 2;
output.GyroAngleRate = data[checkedValue + 1] * 256 + data[checkedValue]; output.GyroAngleRate = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 5; // 3 unsued checkedValue += 5; // 3 unsued
} else if (data[checkedValue] == 0x05) { }
else if (data[checkedValue] == 0x05)
{
checkedValue++; checkedValue++;
if (data[checkedValue] != 0x06) if (data[checkedValue] != 0x06)
return -5; return -5;
@@ -359,7 +397,9 @@ int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char *data) {
output.CliffSensorLeft = output.CliffSensorLeft =
data[checkedValue + 1] * 256 + data[checkedValue]; data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; checkedValue += 2;
} else if (data[checkedValue] == 0x06) { }
else if (data[checkedValue] == 0x06)
{
checkedValue++; checkedValue++;
if (data[checkedValue] != 0x02) if (data[checkedValue] != 0x02)
return -6; return -6;
@@ -368,8 +408,9 @@ int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char *data) {
checkedValue++; checkedValue++;
output.wheelCurrentRight = data[checkedValue]; output.wheelCurrentRight = data[checkedValue];
checkedValue++; checkedValue++;
}
} else if (data[checkedValue] == 0x0A) { else if (data[checkedValue] == 0x0A)
{
checkedValue++; checkedValue++;
if (data[checkedValue] != 0x04) if (data[checkedValue] != 0x04)
return -7; return -7;
@@ -380,8 +421,9 @@ int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char *data) {
checkedValue++; checkedValue++;
output.extraInfo.HardwareVersionMajor = data[checkedValue]; output.extraInfo.HardwareVersionMajor = data[checkedValue];
checkedValue += 2; checkedValue += 2;
}
} else if (data[checkedValue] == 0x0B) { else if (data[checkedValue] == 0x0B)
{
checkedValue++; checkedValue++;
if (data[checkedValue] != 0x04) if (data[checkedValue] != 0x04)
return -8; return -8;
@@ -392,8 +434,9 @@ int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char *data) {
checkedValue++; checkedValue++;
output.extraInfo.FirmwareVersionMajor = data[checkedValue]; output.extraInfo.FirmwareVersionMajor = data[checkedValue];
checkedValue += 2; checkedValue += 2;
}
} else if (data[checkedValue] == 0x0D) { else if (data[checkedValue] == 0x0D)
{
checkedValue++; checkedValue++;
if (data[checkedValue] % 2 != 0) if (data[checkedValue] % 2 != 0)
return -9; return -9;
@@ -404,7 +447,8 @@ int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char *data) {
checkedValue++; checkedValue++;
output.gyroData.reserve(howmanyFrames); output.gyroData.reserve(howmanyFrames);
output.gyroData.clear(); output.gyroData.clear();
for (int hk = 0; hk < howmanyFrames; hk++) { for (int hk = 0; hk < howmanyFrames; hk++)
{
TRawGyroData temp; TRawGyroData temp;
temp.x = data[checkedValue + 1] * 256 + data[checkedValue]; temp.x = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; checkedValue += 2;
@@ -414,7 +458,9 @@ int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char *data) {
checkedValue += 2; checkedValue += 2;
output.gyroData.push_back(temp); output.gyroData.push_back(temp);
} }
} else if (data[checkedValue] == 0x10) { }
else if (data[checkedValue] == 0x10)
{
checkedValue++; checkedValue++;
if (data[checkedValue] != 0x10) if (data[checkedValue] != 0x10)
return -10; return -10;
@@ -429,8 +475,9 @@ int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char *data) {
checkedValue += 2; checkedValue += 2;
output.analogInputCh3 = data[checkedValue + 1] * 256 + data[checkedValue]; output.analogInputCh3 = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 8; // 2+6 checkedValue += 8; // 2+6
}
} else if (data[checkedValue] == 0x13) { else if (data[checkedValue] == 0x13)
{
checkedValue++; checkedValue++;
if (data[checkedValue] != 0x0C) if (data[checkedValue] != 0x0C)
return -11; return -11;
@@ -450,7 +497,9 @@ int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char *data) {
data[checkedValue + 1] * 256 + data[checkedValue + 1] * 256 +
data[checkedValue]; data[checkedValue];
checkedValue += 4; checkedValue += 4;
} else { }
else
{
checkedValue++; checkedValue++;
checkedValue += data[checkedValue] + 1; checkedValue += data[checkedValue] + 1;
} }
@@ -458,19 +507,25 @@ int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char *data) {
return 0; return 0;
} }
long double CKobuki::gyroToRad(signed short GyroAngle) { long double CKobuki::gyroToRad(signed short GyroAngle)
{
long double ret; long double rad;
if (GyroAngle < 0) { if (GyroAngle < 0)
ret = GyroAngle + 360; {
} else { rad = GyroAngle + 360;
ret = GyroAngle;
} }
return (long double)ret * PI / 180.0; else
{
rad = GyroAngle;
}
return (long double)rad * PI / 180.0;
} }
long CKobuki::loop(void *user_data, TKobukiData &Kobuki_data) { long CKobuki::loop(void *user_data, TKobukiData &Kobuki_data)
if (iterationCount == 0) { {
if (iterationCount == 0)
{
prevLeftEncoder = Kobuki_data.EncoderLeft; prevLeftEncoder = Kobuki_data.EncoderLeft;
prevRightEncoder = Kobuki_data.EncoderRight; prevRightEncoder = Kobuki_data.EncoderRight;
prevTimestamp = Kobuki_data.timestamp; prevTimestamp = Kobuki_data.timestamp;
@@ -479,27 +534,35 @@ long CKobuki::loop(void *user_data, TKobukiData &Kobuki_data) {
} }
int dLeft; int dLeft;
if (abs(Kobuki_data.EncoderLeft - prevLeftEncoder) > 32000) { if (abs(Kobuki_data.EncoderLeft - prevLeftEncoder) > 32000)
{
dLeft = Kobuki_data.EncoderLeft - prevLeftEncoder + dLeft = Kobuki_data.EncoderLeft - prevLeftEncoder +
(Kobuki_data.EncoderLeft > prevLeftEncoder ? -65536 : +65536); (Kobuki_data.EncoderLeft > prevLeftEncoder ? -65536 : +65536);
} else { }
else
{
dLeft = Kobuki_data.EncoderLeft - prevLeftEncoder; dLeft = Kobuki_data.EncoderLeft - prevLeftEncoder;
} }
int dRight; int dRight;
if (abs(Kobuki_data.EncoderRight - prevRightEncoder) > 32000) { if (abs(Kobuki_data.EncoderRight - prevRightEncoder) > 32000)
{
dRight = Kobuki_data.EncoderRight - prevRightEncoder + dRight = Kobuki_data.EncoderRight - prevRightEncoder +
(Kobuki_data.EncoderRight > prevRightEncoder ? -65536 : +65536); (Kobuki_data.EncoderRight > prevRightEncoder ? -65536 : +65536);
} else { }
else
{
dRight = Kobuki_data.EncoderRight - prevRightEncoder; dRight = Kobuki_data.EncoderRight - prevRightEncoder;
} }
long double dGyroTheta = prevGyroTheta - gyroToRad(Kobuki_data.GyroAngle); long double dGyroTheta = prevGyroTheta - gyroToRad(Kobuki_data.GyroAngle);
if (dGyroTheta > PI) { if (dGyroTheta > PI)
{
dGyroTheta -= 2 * PI; dGyroTheta -= 2 * PI;
} }
if (dGyroTheta < -1 * PI) { if (dGyroTheta < -1 * PI)
{
dGyroTheta += 2 * PI; dGyroTheta += 2 * PI;
} }
@@ -510,9 +573,12 @@ long CKobuki::loop(void *user_data, TKobukiData &Kobuki_data) {
long double mLeft = dLeft * tickToMeter; long double mLeft = dLeft * tickToMeter;
long double mRight = dRight * tickToMeter; long double mRight = dRight * tickToMeter;
if (mLeft == mRight) { if (mLeft == mRight)
{
x = x + mRight; x = x + mRight;
} else { }
else
{
x = x + (b * (mRight + mLeft)) / (2 * (mRight - mLeft)) * x = x + (b * (mRight + mLeft)) / (2 * (mRight - mLeft)) *
(sin((mRight - mLeft) / b + theta) - sin(theta)); (sin((mRight - mLeft) / b + theta) - sin(theta));
y = y + (b * (mRight + mLeft)) / (2 * (mRight - mLeft)) * y = y + (b * (mRight + mLeft)) / (2 * (mRight - mLeft)) *
@@ -564,7 +630,8 @@ long CKobuki::loop(void *user_data, TKobukiData &Kobuki_data) {
// povie kobukimu ze ma ist niekolko metrov dopredu alebo dozadu, rozhoduje // povie kobukimu ze ma ist niekolko metrov dopredu alebo dozadu, rozhoduje
// znamienko funkcia kompenzuje chodenie rovno pomocou regulatora, interne // znamienko funkcia kompenzuje chodenie rovno pomocou regulatora, interne
// vyuziva setArcSpeed a ako spatnu vazbu pouziva data z enkoderov // vyuziva setArcSpeed a ako spatnu vazbu pouziva data z enkoderov
void CKobuki::goStraight(long double distance) { void CKobuki::goStraight(long double distance)
{
long double u_translation = 0; // riadena velicina, rychlost robota pri pohybe long double u_translation = 0; // riadena velicina, rychlost robota pri pohybe
long double w_translation = distance; // pozadovana hodnota long double w_translation = distance; // pozadovana hodnota
@@ -586,7 +653,8 @@ void CKobuki::goStraight(long double distance) {
long i = 5; long i = 5;
while (fabs(x - w_translation) > 0.005 && x < w_translation) { while (fabs(x - w_translation) > 0.005 && x < w_translation)
{
e_translation = w_translation - x; e_translation = w_translation - x;
u_translation = Kp_translation * e_translation; u_translation = Kp_translation * e_translation;
@@ -601,15 +669,18 @@ void CKobuki::goStraight(long double distance) {
u_translation = lower_thresh_translation; u_translation = lower_thresh_translation;
// rewrite starting speed with line // rewrite starting speed with line
if (i < u_translation) { if (i < u_translation)
{
u_translation = i; u_translation = i;
} }
if (fabs(u_rotation) > 32767) { if (fabs(u_rotation) > 32767)
{
u_rotation = -32767; u_rotation = -32767;
} }
if (u_rotation == 0) { if (u_rotation == 0)
{
u_rotation = -32767; u_rotation = -32767;
} }
@@ -624,9 +695,10 @@ void CKobuki::goStraight(long double distance) {
/// the method performs the rotation, it rotates using the regulator, the /// the method performs the rotation, it rotates using the regulator, the
/// gyroscope serves as feedback, because it is much more accurate than encoders /// gyroscope serves as feedback, because it is much more accurate than encoders
void CKobuki::doRotation(long double th) { void CKobuki::doRotation(long double th)
long double u = 0; // riadena velicina, uhlova rychlost robota pri pohybe {
long double w = th; // pozadovana hodnota v radianoch long double 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 Kp = PI;
long double e = 0; long double e = 0;
int thresh = PI / 2; int thresh = PI / 2;
@@ -638,8 +710,10 @@ void CKobuki::doRotation(long double th) {
long double i = 0; long double i = 0;
if (w > 0) { if (w > 0)
while (gyroTheta < w) { {
while (gyroTheta < w)
{
e = w - gyroTheta; e = w - gyroTheta;
u = Kp * e; u = Kp * e;
@@ -648,7 +722,8 @@ void CKobuki::doRotation(long double th) {
if (u < 0.4) if (u < 0.4)
u = 0.4; u = 0.4;
if (i < u) { if (i < u)
{
u = i; u = i;
} }
@@ -657,8 +732,11 @@ void CKobuki::doRotation(long double th) {
usleep(25 * 1000); usleep(25 * 1000);
i = i + 0.1; i = i + 0.1;
} }
} else { }
while (gyroTheta > w) { else
{
while (gyroTheta > w)
{
e = w - gyroTheta; e = w - gyroTheta;
u = Kp * e * -1; u = Kp * e * -1;
@@ -667,7 +745,8 @@ void CKobuki::doRotation(long double th) {
if (u < 0.4) if (u < 0.4)
u = 0.4; u = 0.4;
if (i < u) { if (i < u)
{
u = i; u = i;
} }
@@ -684,9 +763,10 @@ void CKobuki::doRotation(long double th) {
usleep(25 * 1000); usleep(25 * 1000);
} }
// kombinuje navadzanie na suradnicu a rotaciu o uhol, realizuje presun na // combines navigation to a coordinate and rotation by an angle, performs movement to
// zvolenu suradnicu v suradnicovom systeme robota // the selected coordinate in the robot's coordinate system
void CKobuki::goToXy(long double xx, long double yy) { void CKobuki::goToXy(long double xx, long double yy)
{
long double th; long double th;
yy = yy * -1; yy = yy * -1;