From a8fcf0e0f8fd5d916dab3cd525b6733b9516e93d Mon Sep 17 00:00:00 2001 From: "ishak jmilou.ishak" Date: Thu, 10 Oct 2024 12:24:15 +0200 Subject: [PATCH] added curly brackets --- src/C++/Driver/src/CKobuki.cpp | 997 ++++++++++++++++----------------- 1 file changed, 471 insertions(+), 526 deletions(-) diff --git a/src/C++/Driver/src/CKobuki.cpp b/src/C++/Driver/src/CKobuki.cpp index 9b4687d..57518be 100755 --- a/src/C++/Driver/src/CKobuki.cpp +++ b/src/C++/Driver/src/CKobuki.cpp @@ -4,302 +4,281 @@ #include #include - // plot p; static std::vector vectorX; static std::vector vectorY; static std::vector 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; - } +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); + 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_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_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; + 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; + 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; - } +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 + 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); + 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); +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); + 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 + 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); + tcsetattr(HCom, TCSANOW, &settings); // apply the settings*/ + tcflush(HCom, TCOFLUSH); - printf("Kobuki pripojeny\n"); - return HCom; - } + 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 - { +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); + 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 - if (Pocet == 1 && buffer[0] == 0x55) - { - // precitame dlzku - Pocet = read(HCom, buffer, 1); + // 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; + // 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); + 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)); + 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> 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]; - +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, 14); + pocet = write(HCom, &message, 8); + } } -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]; +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); + uint32_t pocet; + pocet = write(HCom, &message, 14); } -void CKobuki::setArcSpeed(int mmpersec, int radius) -{ - if (radius == 0) - { - setTranslationSpeed(mmpersec); - return; +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); - 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; + // maximalne moze trvat callback funkcia 20 ms, ak by trvala viac, nestihame + // citat + if (ok == 0) { + loop(userData, parser.data); } -} - -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; + free(message); + } + return 0; } // int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char *data) @@ -320,25 +299,20 @@ int CKobuki::measure() // 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]; +// 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]; @@ -369,10 +343,10 @@ int CKobuki::measure() // 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 +// 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) // { @@ -457,16 +431,14 @@ int CKobuki::measure() // 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 +// 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) // { @@ -474,17 +446,20 @@ int CKobuki::measure() // if (data[checkedValue] != 0x0C) // return -11; // checkedValue++; -// output.extraInfo.UDID0 = data[checkedValue + 3] * 256 * 256 * 256 + +// 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 + +// 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 + +// output.extraInfo.UDID2 = data[checkedValue + 3] * 256 * 256 * 256 +// + // data[checkedValue + 2] * 256 * 256 + // data[checkedValue + 1] * 256 + // data[checkedValue]; @@ -499,135 +474,116 @@ int CKobuki::measure() // return 0; // } -long double CKobuki::gyroToRad(signed short GyroAngle) -{ +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 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); - +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++; + } - // std::cout << "X: " << x - // << " Y: " << y - // << " Theta: " << theta - // << "Gyro theta:" << gyroTheta - // << std::endl; + 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; + } - static long counter = 0; + 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; + } - vectorX.push_back(gx); - vectorY.push_back(gy); - vectorGyroTheta.push_back(gyroTheta); + long double dGyroTheta = prevGyroTheta - gyroToRad(Kobuki_data.GyroAngle); - // if (counter % 100 == 0) { - // p.plot_data(vectorY, vectorX); - // } - counter++; + if (dGyroTheta > PI) { + dGyroTheta -= 2 * PI; + } + if (dGyroTheta < -1 * PI) { + dGyroTheta += 2 * PI; + } - return 0; + 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 +// 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; @@ -636,150 +592,139 @@ void CKobuki::goStraight(long double distance){ 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; + 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; + 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; + 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; + 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; + // 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; + // rewrite starting speed with line + if (i < u_translation) { + u_translation = i; } - this->setTranslationSpeed(0); + + 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; +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; + theta = 0; + x = 0; + y = 0; + gyroTheta = 0; - long double i = 0; + long double i = 0; - if (w > 0) - { - while (gyroTheta < w) - { - e = w - gyroTheta; - u = Kp * e; + 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 (u > thresh) { + u = thresh; + } + if (u < 0.4) { + u = 0.4; + } + if (i < u) { + u = i; + } - if (i < u) - { - u = i; - } - - std::cout << "Angle: " << gyroTheta << " required:" << w << std::endl; - this->setRotationSpeed(-1 * u); - usleep(25 * 1000); - i = i + 0.1; - } + 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; + } else { + while (gyroTheta > w) { + e = w - gyroTheta; + u = Kp * e * -1; - if (u > thresh) - u = thresh; - if (u < 0.4) - u = 0.4; + if (u > thresh) + u = thresh; + if (u < 0.4) + u = 0.4; - if (i < u) - { - u = i; - } + 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 << "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); + 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; +// 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; + yy = yy * -1; - th = atan2(yy, xx); - doRotation(th); + th = atan2(yy, xx); + doRotation(th); - long double s = sqrt(pow(xx, 2) + pow(yy, 2)); + long double s = sqrt(pow(xx, 2) + pow(yy, 2)); - // resetnem suradnicovu sustavu robota - x = 0; - y = 0; - iterationCount = 0; - theta = 0; + // resetnem suradnicovu sustavu robota + x = 0; + y = 0; + iterationCount = 0; + theta = 0; - // std::cout << "mam prejst: " << s << "[m]" << std::endl; + // std::cout << "mam prejst: " << s << "[m]" << std::endl; - goStraight(s); + goStraight(s); - usleep(25 * 1000); - return; + usleep(25 * 1000); + return; } void CKobuki::forward(int speedvalue, long double distance) { @@ -789,22 +734,22 @@ void CKobuki::forward(int speedvalue, long double distance) { 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 -}; + 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]; + // 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;