mirror of
https://gitlab.fdmci.hva.nl/technische-informatica-sm3/ti-projectten/rooziinuubii79.git
synced 2025-08-03 20:04:58 +00:00
formatted the code
This commit is contained in:
ishak jmilou.ishak
@@ -1,21 +1,20 @@
|
||||
#include "CKobuki.h"
|
||||
#include "termios.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)
|
||||
{
|
||||
int set_interface_attribs2(int fd, int speed, int parity) {
|
||||
struct termios tty;
|
||||
memset(&tty, 0, sizeof tty);
|
||||
if (tcgetattr (fd, &tty) != 0)
|
||||
{
|
||||
if (tcgetattr(fd, &tty) != 0) {
|
||||
printf("error %d from tcgetattr", errno);
|
||||
return -1;
|
||||
}
|
||||
@@ -33,7 +32,8 @@ int set_interface_attribs2 (int fd, int speed, int parity)
|
||||
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
|
||||
@@ -42,20 +42,17 @@ int set_interface_attribs2 (int fd, int speed, int parity)
|
||||
tty.c_cflag &= ~CSTOPB;
|
||||
tty.c_cflag &= ~CRTSCTS;
|
||||
|
||||
if (tcsetattr (fd, TCSANOW, &tty) != 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)
|
||||
{
|
||||
void set_blocking2(int fd, int should_block) {
|
||||
struct termios tty;
|
||||
memset(&tty, 0, sizeof tty);
|
||||
if (tcgetattr (fd, &tty) != 0)
|
||||
{
|
||||
if (tcgetattr(fd, &tty) != 0) {
|
||||
printf("error %d from tggetattr", errno);
|
||||
return;
|
||||
}
|
||||
@@ -67,24 +64,16 @@ void set_blocking2 (int fd, int should_block)
|
||||
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);
|
||||
|
||||
if ( HCom== -1 )
|
||||
{
|
||||
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)
|
||||
} 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);
|
||||
@@ -101,14 +90,12 @@ int CKobuki::connect(char * comportT)
|
||||
tcsetattr(HCom, TCSANOW, &settings); // apply the settings*/
|
||||
tcflush(HCom, TCOFLUSH);
|
||||
|
||||
|
||||
printf("Kobuki pripojeny\n");
|
||||
return HCom;
|
||||
}
|
||||
}
|
||||
|
||||
unsigned char * CKobuki::readKobukiMessage()
|
||||
{
|
||||
unsigned char *CKobuki::readKobukiMessage() {
|
||||
unsigned char buffer[1];
|
||||
ssize_t Pocet;
|
||||
buffer[0] = 0;
|
||||
@@ -118,8 +105,7 @@ unsigned char * CKobuki::readKobukiMessage()
|
||||
Pocet = read(HCom, buffer, 1);
|
||||
} while (buffer[0] != 0xAA);
|
||||
// mame zaciatok spravy (asi)
|
||||
if (Pocet == 1 && buffer[0] == 0xAA)
|
||||
{
|
||||
if (Pocet == 1 && buffer[0] == 0xAA) {
|
||||
// citame dalsi byte
|
||||
do {
|
||||
|
||||
@@ -127,25 +113,21 @@ unsigned char * CKobuki::readKobukiMessage()
|
||||
|
||||
} while (Pocet != 1); // na linuxe -1 na windowse 0
|
||||
|
||||
|
||||
|
||||
// a ak je to druhy byte hlavicky
|
||||
if (Pocet == 1 && buffer[0] == 0x55)
|
||||
{
|
||||
if (Pocet == 1 && buffer[0] == 0x55) {
|
||||
// precitame dlzku
|
||||
Pocet = read(HCom, buffer, 1);
|
||||
|
||||
// ReadFile(hCom, buffer, 1, &Pocet, NULL);
|
||||
if (Pocet == 1)
|
||||
{
|
||||
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));
|
||||
unsigned char *outputBuffer =
|
||||
(unsigned char *)calloc(readLenght + 4, sizeof(char));
|
||||
outputBuffer[0] = buffer[0];
|
||||
int pct = 0;
|
||||
|
||||
do
|
||||
{
|
||||
do {
|
||||
Pocet = 0;
|
||||
int readpoc = (readLenght + 1 - pct);
|
||||
Pocet = read(HCom, outputBuffer + 1 + pct, readpoc);
|
||||
@@ -167,25 +149,23 @@ unsigned char * CKobuki::readKobukiMessage()
|
||||
return null_buffer;
|
||||
}
|
||||
|
||||
int CKobuki::checkChecksum(unsigned char * data)
|
||||
{//najprv hlavicku
|
||||
int CKobuki::checkChecksum(unsigned char *data) { // najprv hlavicku
|
||||
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];
|
||||
}
|
||||
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};
|
||||
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...
|
||||
// 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};
|
||||
@@ -194,83 +174,106 @@ void CKobuki::setPower(int value){
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
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];
|
||||
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)
|
||||
{
|
||||
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];
|
||||
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)
|
||||
{
|
||||
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];
|
||||
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)
|
||||
{
|
||||
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];
|
||||
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)
|
||||
{
|
||||
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);
|
||||
pthread_result =
|
||||
pthread_create(&threadHandle, NULL, KobukiProcess, (void *)this);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
int CKobuki::measure()
|
||||
{
|
||||
while (stopVlakno==0)
|
||||
{
|
||||
int CKobuki::measure() {
|
||||
while (stopVlakno == 0) {
|
||||
unsigned char *message = readKobukiMessage();
|
||||
if (message == NULL)
|
||||
{
|
||||
if (message == NULL) {
|
||||
// printf("vratil null message\n");
|
||||
continue;
|
||||
}
|
||||
int ok = parseKobukiMessage(data, message);
|
||||
|
||||
//maximalne moze trvat callback funkcia 20 ms, ak by trvala viac, nestihame citat
|
||||
if (ok == 0)
|
||||
{
|
||||
// maximalne moze trvat callback funkcia 20 ms, ak by trvala viac, nestihame
|
||||
// citat
|
||||
if (ok == 0) {
|
||||
loop(userData, data);
|
||||
}
|
||||
free(message);
|
||||
@@ -278,8 +281,7 @@ int CKobuki::measure()
|
||||
return 0;
|
||||
}
|
||||
|
||||
int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char * data)
|
||||
{
|
||||
int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char *data) {
|
||||
int rtrnvalue = checkChecksum(data);
|
||||
// ak je zly checksum,tak kaslat na to
|
||||
if (rtrnvalue != 0)
|
||||
@@ -287,11 +289,9 @@ int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char * data)
|
||||
|
||||
int checkedValue = 1;
|
||||
// kym neprejdeme celu dlzku
|
||||
while (checkedValue < data[0])
|
||||
{
|
||||
while (checkedValue < data[0]) {
|
||||
// basic data subload
|
||||
if (data[checkedValue] == 0x01)
|
||||
{
|
||||
if (data[checkedValue] == 0x01) {
|
||||
checkedValue++;
|
||||
if (data[checkedValue] != 0x0F)
|
||||
return -1;
|
||||
@@ -325,9 +325,7 @@ int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char * data)
|
||||
checkedValue++;
|
||||
output.overCurrent = data[checkedValue];
|
||||
checkedValue++;
|
||||
}
|
||||
else if (data[checkedValue] == 0x03)
|
||||
{
|
||||
} else if (data[checkedValue] == 0x03) {
|
||||
checkedValue++;
|
||||
if (data[checkedValue] != 0x03)
|
||||
return -3;
|
||||
@@ -338,9 +336,7 @@ int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char * data)
|
||||
checkedValue++;
|
||||
output.IRSensorLeft = data[checkedValue];
|
||||
checkedValue++;
|
||||
}
|
||||
else if (data[checkedValue] == 0x04)
|
||||
{
|
||||
} else if (data[checkedValue] == 0x04) {
|
||||
checkedValue++;
|
||||
if (data[checkedValue] != 0x07)
|
||||
return -4;
|
||||
@@ -349,22 +345,21 @@ int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char * data)
|
||||
checkedValue += 2;
|
||||
output.GyroAngleRate = data[checkedValue + 1] * 256 + data[checkedValue];
|
||||
checkedValue += 5; // 3 unsued
|
||||
}
|
||||
else if (data[checkedValue] == 0x05)
|
||||
{
|
||||
} else if (data[checkedValue] == 0x05) {
|
||||
checkedValue++;
|
||||
if (data[checkedValue] != 0x06)
|
||||
return -5;
|
||||
checkedValue++;
|
||||
output.CliffSensorRight = data[checkedValue + 1] * 256 + data[checkedValue];
|
||||
output.CliffSensorRight =
|
||||
data[checkedValue + 1] * 256 + data[checkedValue];
|
||||
checkedValue += 2;
|
||||
output.CliffSensorCenter = data[checkedValue + 1] * 256 + data[checkedValue];
|
||||
output.CliffSensorCenter =
|
||||
data[checkedValue + 1] * 256 + data[checkedValue];
|
||||
checkedValue += 2;
|
||||
output.CliffSensorLeft = data[checkedValue + 1] * 256 + data[checkedValue];
|
||||
output.CliffSensorLeft =
|
||||
data[checkedValue + 1] * 256 + data[checkedValue];
|
||||
checkedValue += 2;
|
||||
}
|
||||
else if (data[checkedValue] == 0x06)
|
||||
{
|
||||
} else if (data[checkedValue] == 0x06) {
|
||||
checkedValue++;
|
||||
if (data[checkedValue] != 0x02)
|
||||
return -6;
|
||||
@@ -374,9 +369,7 @@ int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char * data)
|
||||
output.wheelCurrentRight = data[checkedValue];
|
||||
checkedValue++;
|
||||
|
||||
}
|
||||
else if (data[checkedValue] == 0x0A)
|
||||
{
|
||||
} else if (data[checkedValue] == 0x0A) {
|
||||
checkedValue++;
|
||||
if (data[checkedValue] != 0x04)
|
||||
return -7;
|
||||
@@ -388,9 +381,7 @@ int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char * data)
|
||||
output.extraInfo.HardwareVersionMajor = data[checkedValue];
|
||||
checkedValue += 2;
|
||||
|
||||
}
|
||||
else if (data[checkedValue] == 0x0B)
|
||||
{
|
||||
} else if (data[checkedValue] == 0x0B) {
|
||||
checkedValue++;
|
||||
if (data[checkedValue] != 0x04)
|
||||
return -8;
|
||||
@@ -402,9 +393,7 @@ int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char * data)
|
||||
output.extraInfo.FirmwareVersionMajor = data[checkedValue];
|
||||
checkedValue += 2;
|
||||
|
||||
}
|
||||
else if (data[checkedValue] == 0x0D)
|
||||
{
|
||||
} else if (data[checkedValue] == 0x0D) {
|
||||
checkedValue++;
|
||||
if (data[checkedValue] % 2 != 0)
|
||||
return -9;
|
||||
@@ -415,8 +404,7 @@ int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char * data)
|
||||
checkedValue++;
|
||||
output.gyroData.reserve(howmanyFrames);
|
||||
output.gyroData.clear();
|
||||
for (int hk = 0; hk < howmanyFrames; hk++)
|
||||
{
|
||||
for (int hk = 0; hk < howmanyFrames; hk++) {
|
||||
TRawGyroData temp;
|
||||
temp.x = data[checkedValue + 1] * 256 + data[checkedValue];
|
||||
checkedValue += 2;
|
||||
@@ -426,9 +414,7 @@ int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char * data)
|
||||
checkedValue += 2;
|
||||
output.gyroData.push_back(temp);
|
||||
}
|
||||
}
|
||||
else if (data[checkedValue] == 0x10)
|
||||
{
|
||||
} else if (data[checkedValue] == 0x10) {
|
||||
checkedValue++;
|
||||
if (data[checkedValue] != 0x10)
|
||||
return -10;
|
||||
@@ -444,23 +430,27 @@ int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char * data)
|
||||
output.analogInputCh3 = data[checkedValue + 1] * 256 + data[checkedValue];
|
||||
checkedValue += 8; // 2+6
|
||||
|
||||
|
||||
}
|
||||
else if (data[checkedValue] == 0x13)
|
||||
{
|
||||
} 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];
|
||||
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];
|
||||
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];
|
||||
output.extraInfo.UDID2 = data[checkedValue + 3] * 256 * 256 * 256 +
|
||||
data[checkedValue + 2] * 256 * 256 +
|
||||
data[checkedValue + 1] * 256 +
|
||||
data[checkedValue];
|
||||
checkedValue += 4;
|
||||
}
|
||||
else
|
||||
{
|
||||
} else {
|
||||
checkedValue++;
|
||||
checkedValue += data[checkedValue] + 1;
|
||||
}
|
||||
@@ -468,28 +458,17 @@ int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char * data)
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
long double CKobuki::gyroToRad(signed short GyroAngle) {
|
||||
|
||||
long double ret;
|
||||
if (GyroAngle < 0) {
|
||||
ret = GyroAngle + 36000;
|
||||
}
|
||||
else {
|
||||
} else {
|
||||
ret = GyroAngle;
|
||||
}
|
||||
return (long double)ret * PI / 18000.0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
long CKobuki::loop(void *user_data, TKobukiData &Kobuki_data) {
|
||||
if (iterationCount == 0) {
|
||||
prevLeftEncoder = Kobuki_data.EncoderLeft;
|
||||
@@ -501,17 +480,17 @@ long CKobuki::loop(void *user_data, TKobukiData &Kobuki_data) {
|
||||
|
||||
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 +
|
||||
(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 +
|
||||
(Kobuki_data.EncoderRight > prevRightEncoder ? -65536 : +65536);
|
||||
} else {
|
||||
dRight = Kobuki_data.EncoderRight - prevRightEncoder;
|
||||
}
|
||||
|
||||
@@ -534,8 +513,10 @@ long CKobuki::loop(void *user_data, TKobukiData &Kobuki_data) {
|
||||
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));
|
||||
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;
|
||||
}
|
||||
|
||||
@@ -545,33 +526,27 @@ long CKobuki::loop(void *user_data, TKobukiData &Kobuki_data) {
|
||||
|
||||
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);
|
||||
|
||||
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);
|
||||
@@ -586,12 +561,9 @@ long CKobuki::loop(void *user_data, TKobukiData &Kobuki_data) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
// povie kobukimu ze ma ist niekolko metrov dopredu alebo dozadu, rozhoduje znamienko
|
||||
// funkcia kompenzuje chodenie rovno pomocou regulatora, interne vyuziva setArcSpeed a
|
||||
// ako spatnu vazbu pouziva data z enkoderov
|
||||
// povie kobukimu ze ma ist niekolko metrov dopredu alebo dozadu, rozhoduje
|
||||
// znamienko funkcia kompenzuje chodenie rovno pomocou regulatora, interne
|
||||
// vyuziva setArcSpeed a ako spatnu vazbu pouziva data z enkoderov
|
||||
void CKobuki::goStraight(long double distance) {
|
||||
long double u_translation = 0; // riadena velicina, rychlost robota pri pohybe
|
||||
long double w_translation = distance; // pozadovana hodnota
|
||||
@@ -619,8 +591,8 @@ void CKobuki::goStraight(long double distance){
|
||||
u_translation = Kp_translation * e_translation;
|
||||
|
||||
e_rotation = w_rotation - theta;
|
||||
if (!e_rotation == 0) u_rotation = Kp_rotation / e_rotation;
|
||||
|
||||
if (!e_rotation == 0)
|
||||
u_rotation = Kp_rotation / e_rotation;
|
||||
|
||||
// limit translation speed
|
||||
if (u_translation > upper_thresh_translation)
|
||||
@@ -650,10 +622,8 @@ void CKobuki::goStraight(long double distance){
|
||||
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
|
||||
/// 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; // riadena velicina, uhlova rychlost robota pri pohybe
|
||||
long double w = th; // pozadovana hodnota v radianoch
|
||||
@@ -673,8 +643,10 @@ void CKobuki::doRotation(long double th) {
|
||||
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;
|
||||
@@ -685,14 +657,15 @@ void CKobuki::doRotation(long double th) {
|
||||
usleep(25 * 1000);
|
||||
i = i + 0.1;
|
||||
}
|
||||
}
|
||||
else {
|
||||
} 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;
|
||||
@@ -711,9 +684,8 @@ void CKobuki::doRotation(long double th) {
|
||||
usleep(25 * 1000);
|
||||
}
|
||||
|
||||
|
||||
// kombinuje navadzanie na suradnicu a rotaciu o uhol, realizuje presun na zvolenu suradnicu
|
||||
// v suradnicovom systeme robota
|
||||
// kombinuje navadzanie na suradnicu a rotaciu o uhol, realizuje presun na
|
||||
// zvolenu suradnicu v suradnicovom systeme robota
|
||||
void CKobuki::goToXy(long double xx, long double yy) {
|
||||
long double th;
|
||||
|
||||
@@ -737,4 +709,3 @@ void CKobuki::goToXy(long double xx, long double yy) {
|
||||
usleep(25 * 1000);
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user