mirror of
https://gitlab.fdmci.hva.nl/technische-informatica-sm3/ti-projectten/rooziinuubii79.git
synced 2025-08-03 20:04:58 +00:00
813 lines
26 KiB
C++
Executable File
813 lines
26 KiB
C++
Executable File
#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;
|
|
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;
|
|
}
|
|
|
|
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);
|
|
}
|