Smikkelbakje/J2S1-Kobuki
1
0
Fork 0
mirror of https://gitlab.fdmci.hva.nl/technische-informatica-sm3/ti-projectten/rooziinuubii79.git synced 2025-08-04 04:14:58 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'origin/main'

Browse Source
This commit is contained in:
Sam Hos
2024-10-01 16:40:16 +02:00
parent 2e0a27c732 163e7d4b7c
commit dd712f6c4f
1 changed files with 584 additions and 610 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,91 +1,80 @@
#include "CKobuki.h" #include "CKobuki.h"
#include "termios.h"
#include "errno.h" #include "errno.h"
#include "termios.h"
#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;
} }
cfsetospeed (&tty, speed); cfsetospeed(&tty, speed);
cfsetispeed (&tty, speed); cfsetispeed(&tty, speed);
tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8; // 8-bit chars tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8; // 8-bit chars
// disable IGNBRK for mismatched speed tests; otherwise receive break // disable IGNBRK for mismatched speed tests; otherwise receive break
// as \000 chars // as \000 chars
//tty.c_iflag &= ~IGNBRK; // disable break processing // tty.c_iflag &= ~IGNBRK; // disable break processing
tty.c_lflag = 0; // no signaling chars, no echo, tty.c_lflag = 0; // no signaling chars, no echo,
// no canonical processing // no canonical processing
tty.c_oflag = 0; // no remapping, no delays tty.c_oflag = 0; // no remapping, no delays
tty.c_cc[VMIN] = 0; // read doesn't block tty.c_cc[VMIN] = 0; // read doesn't block
tty.c_cc[VTIME] = 5; // 0.5 seconds read timeout 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, tty.c_cflag |= (CLOCAL | CREAD); // ignore modem controls,
// enable reading // enable reading
tty.c_cflag &= ~(PARENB | PARODD); // shut off parity tty.c_cflag &= ~(PARENB | PARODD); // shut off parity
tty.c_cflag |= parity; tty.c_cflag |= 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;
} }
tty.c_cc[VMIN] = should_block ? 1 : 0; tty.c_cc[VMIN] = should_block ? 1 : 0;
tty.c_cc[VTIME] = 5; // 0.5 seconds read timeout tty.c_cc[VTIME] = 5; // 0.5 seconds read timeout
if (tcsetattr (fd, TCSANOW, &tty) != 0) if (tcsetattr(fd, TCSANOW, &tty) != 0)
printf ("error %d setting term attributes", errno); printf("error %d setting term attributes", errno);
} }
int CKobuki::connect(char *comportT) {
HCom = open(comportT, O_RDWR | O_NOCTTY | O_NONBLOCK);
if (HCom == -1) {
int CKobuki::connect(char * comportT)
{
HCom= open(comportT,O_RDWR|O_NOCTTY|O_NONBLOCK);
if ( HCom== -1 )
{
printf("Kobuki nepripojeny\n"); printf("Kobuki nepripojeny\n");
return HCom; return HCom;
} } else {
else set_interface_attribs2(HCom, B115200,
{ 0); // set speed to 115,200 bps, 8n1 (no parity)
set_interface_attribs2 (HCom, B115200, 0); // set speed to 115,200 bps, 8n1 (no parity) set_blocking2(HCom, 0); // set no blocking
set_blocking2 (HCom, 0); // set no blocking
/* struct termios settings; /* struct termios settings;
tcgetattr(HCom, &settings); tcgetattr(HCom, &settings);
@@ -101,57 +90,50 @@ int CKobuki::connect(char * comportT)
tcsetattr(HCom, TCSANOW, &settings); // apply the settings*/ tcsetattr(HCom, TCSANOW, &settings); // apply the settings*/
tcflush(HCom, TCOFLUSH); tcflush(HCom, TCOFLUSH);
printf("Kobuki pripojeny\n"); printf("Kobuki pripojeny\n");
return HCom; return HCom;
} }
} }
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
if (Pocet == 1 && buffer[0] == 0x55) {
//a ak je to druhy byte hlavicky
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*)calloc(readLenght+4,sizeof(char)); unsigned char *outputBuffer =
(unsigned char *)calloc(readLenght + 4, sizeof(char));
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);
pct = pct + (Pocet == -1 ? 0 : Pocet); pct = pct + (Pocet == -1 ? 0 : Pocet);
} while (pct != (readLenght+1 )); } while (pct != (readLenght + 1));
// tu si mozeme ceknut co chodi zo serial intefejsu Kobukiho // tu si mozeme ceknut co chodi zo serial intefejsu Kobukiho
// for(int i=0;i<outputBuffer[0]+2;i++) // for(int i=0;i<outputBuffer[0]+2;i++)
@@ -167,110 +149,131 @@ unsigned char * CKobuki::readKobukiMessage()
return null_buffer; return null_buffer;
} }
int CKobuki::checkChecksum(unsigned char * data) int CKobuki::checkChecksum(unsigned char *data) { // najprv hlavicku
{//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;
pocet=write(HCom,&message,8); 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
void CKobuki::setPower(int value){ // napajania aktivne musi byt aj tak v kazdej sprave...
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,
{ 0xf0, 0x00, 0x01, 0x04, mmpersec % 256,
unsigned char message[14] = { 0xaa,0x55,0x0A,0x0c,0x02,0xf0,0x00,0x01,0x04,mmpersec%256,mmpersec>>8,0x00,0x00, 0x00 }; 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]; 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; uint32_t pocet;
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,0x55,0x0A,0x0c,0x02,0xf0,0x00,0x01,0x04,speedvalue % 256,speedvalue >>8,0x01,0x00, 0x00 }; unsigned char message[14] = {0xaa,
message[13] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^ message[7] ^ message[8] ^ message[9] ^ message[10] ^ message[11] ^ message[12]; 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; uint32_t pocet;
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;
} }
int speedvalue = mmpersec * ((radius + (radius>0? 230:-230) )/ 2 ) / radius; int speedvalue =
unsigned char message[14] = { 0xaa,0x55,0x0A,0x0c,0x02,0xf0,0x00,0x01,0x04,speedvalue % 256,speedvalue >>8,radius % 256,radius >>8, 0x00 }; mmpersec * ((radius + (radius > 0 ? 230 : -230)) / 2) / radius;
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,
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; uint32_t pocet;
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,notevalue>>8,duration%256,0x00 }; 0x03, 0x03, notevalue % 256,
message[8] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^ message[7]; notevalue >> 8, duration % 256, 0x00};
message[8] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^
message[7];
uint32_t pocet; uint32_t pocet;
pocet=write(HCom,&message,9); 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); connect(portname);
enableCommands(CommandsEnabled); enableCommands(CommandsEnabled);
userData = userDataL; userData = userDataL;
int pthread_result; 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(); unsigned char *message = readKobukiMessage();
if (message == NULL) if (message == NULL) {
{
// printf("vratil null message\n"); // printf("vratil null message\n");
continue; continue;
} }
int ok=parseKobukiMessage(data,message); int ok = parseKobukiMessage(data, message);
//maximalne moze trvat callback funkcia 20 ms, ak by trvala viac, nestihame citat // maximalne moze trvat callback funkcia 20 ms, ak by trvala viac, nestihame
if (ok == 0) // citat
{ if (ok == 0) {
loop(userData, data); loop(userData, data);
} }
free(message); free(message);
@@ -278,31 +281,28 @@ 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)
return -2; return -2;
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;
checkedValue++; checkedValue++;
output.timestamp = data[checkedValue + 1] * 256 + data[checkedValue]; output.timestamp = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; checkedValue += 2;
output.BumperCenter = data[checkedValue ] && 0x02; output.BumperCenter = data[checkedValue] && 0x02;
output.BumperLeft = data[checkedValue] && 0x04; output.BumperLeft = data[checkedValue] && 0x04;
output.BumperRight = data[checkedValue] && 0x01; output.BumperRight = data[checkedValue] && 0x01;
checkedValue++; checkedValue++;
output.WheelDropLeft= data[checkedValue] && 0x02; output.WheelDropLeft = data[checkedValue] && 0x02;
output.WheelDropRight = data[checkedValue] && 0x01; output.WheelDropRight = data[checkedValue] && 0x01;
checkedValue++; checkedValue++;
output.CliffCenter = data[checkedValue] && 0x02; output.CliffCenter = data[checkedValue] && 0x02;
@@ -313,9 +313,9 @@ int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char * data)
checkedValue += 2; checkedValue += 2;
output.EncoderRight = data[checkedValue + 1] * 256 + data[checkedValue]; output.EncoderRight = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; checkedValue += 2;
output.PWMleft = data[checkedValue] ; output.PWMleft = data[checkedValue];
checkedValue++; checkedValue++;
output.PWMright = data[checkedValue] ; output.PWMright = data[checkedValue];
checkedValue++; checkedValue++;
output.ButtonPress = data[checkedValue]; output.ButtonPress = data[checkedValue];
checkedValue++; checkedValue++;
@@ -325,9 +325,7 @@ 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;
@@ -338,9 +336,7 @@ 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;
@@ -348,35 +344,32 @@ int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char * data)
output.GyroAngle = data[checkedValue + 1] * 256 + data[checkedValue]; output.GyroAngle = data[checkedValue + 1] * 256 + data[checkedValue];
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;
checkedValue++; checkedValue++;
output.CliffSensorRight = data[checkedValue + 1] * 256 + data[checkedValue]; output.CliffSensorRight =
data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; checkedValue += 2;
output.CliffSensorCenter = data[checkedValue + 1] * 256 + data[checkedValue]; output.CliffSensorCenter =
data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; checkedValue += 2;
output.CliffSensorLeft = data[checkedValue + 1] * 256 + data[checkedValue]; output.CliffSensorLeft =
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;
checkedValue++; checkedValue++;
output.wheelCurrentLeft = data[checkedValue]; output.wheelCurrentLeft = data[checkedValue];
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;
@@ -388,9 +381,7 @@ int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char * data)
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;
@@ -402,21 +393,18 @@ int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char * data)
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;
checkedValue++; checkedValue++;
output.frameId = data[checkedValue]; output.frameId = data[checkedValue];
checkedValue++; checkedValue++;
int howmanyFrames = data[checkedValue]/3; int howmanyFrames = data[checkedValue] / 3;
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;
@@ -426,9 +414,7 @@ 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;
@@ -442,25 +428,29 @@ int CKobuki::parseKobukiMessage(TKobukiData &output, unsigned char * data)
output.analogInputCh2 = data[checkedValue + 1] * 256 + data[checkedValue]; output.analogInputCh2 = data[checkedValue + 1] * 256 + data[checkedValue];
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;
checkedValue++; 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; 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; 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; checkedValue += 4;
} } else {
else
{
checkedValue++; checkedValue++;
checkedValue += data[checkedValue] + 1; checkedValue += data[checkedValue] + 1;
} }
@@ -468,28 +458,17 @@ 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 ret;
if (GyroAngle < 0) { if (GyroAngle < 0) {
ret = GyroAngle + 36000; ret = GyroAngle + 360;
} } else {
else {
ret = GyroAngle; ret = GyroAngle;
} }
return (long double) ret*PI/18000.0; return (long double)ret * 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;
@@ -501,76 +480,72 @@ 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 + (Kobuki_data.EncoderLeft > prevLeftEncoder ? -65536 : +65536); dLeft = Kobuki_data.EncoderLeft - prevLeftEncoder +
} (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 + (Kobuki_data.EncoderRight > prevRightEncoder ? -65536 : +65536); dRight = Kobuki_data.EncoderRight - prevRightEncoder +
} (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;
} }
gyroTheta += dGyroTheta; gyroTheta += dGyroTheta;
uint16_t dTimestamp = Kobuki_data.timestamp - prevTimestamp; uint16_t dTimestamp = Kobuki_data.timestamp - prevTimestamp;
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))*(sin((mRight-mLeft)/b + theta) - sin(theta)); x = x + (b * (mRight + mLeft)) / (2 * (mRight - mLeft)) *
y = y + (b*(mRight+mLeft))/(2*(mRight-mLeft))*(cos((mRight-mLeft)/b + theta) - cos(theta)); (sin((mRight - mLeft) / b + theta) - sin(theta));
theta = (mRight-mLeft)/b + 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; displacement = (mRight + mLeft) / 2;
integratedGyroTheta = integratedGyroTheta + dGyroTheta; integratedGyroTheta = integratedGyroTheta + dGyroTheta;
gx = gx + displacement * cos(integratedGyroTheta + dGyroTheta / 2); gx = gx + displacement * cos(integratedGyroTheta + dGyroTheta / 2);
gy = gy + displacement * sin(integratedGyroTheta + dGyroTheta / 2); gy = gy + displacement * sin(integratedGyroTheta + dGyroTheta / 2);
totalLeft += dLeft;
totalRight += dRight;
totalLeft +=dLeft;
totalRight +=dRight;
// ak je suma novej a predchadzajucej vacsia ako 65536 tak to pretieklo? // ak je suma novej a predchadzajucej vacsia ako 65536 tak to pretieklo?
directionL = (prevLeftEncoder < Kobuki_data.EncoderLeft ? 1 : -1); directionL = (prevLeftEncoder < Kobuki_data.EncoderLeft ? 1 : -1);
directionR = (prevRightEncoder < Kobuki_data.EncoderRight ? 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; prevLeftEncoder = Kobuki_data.EncoderLeft;
prevRightEncoder = Kobuki_data.EncoderRight; prevRightEncoder = Kobuki_data.EncoderRight;
prevTimestamp = Kobuki_data.timestamp; prevTimestamp = Kobuki_data.timestamp;
prevGyroTheta = gyroToRad(Kobuki_data.GyroAngle); prevGyroTheta = gyroToRad(Kobuki_data.GyroAngle);
// std::cout << "X: " << x
// std::cout << "X: " << x // << " Y: " << y
// << " Y: " << y // << " Theta: " << theta
// << " Theta: " << theta // << "Gyro theta:" << gyroTheta
// << "Gyro theta:" << gyroTheta // << std::endl;
// << std::endl;
static long counter = 0; static long counter = 0;
@@ -614,13 +589,13 @@ void CKobuki::goStraight(long double distance){
long i = 5; long i = 5;
//send command and hold until robot reaches point //send command and hold until robot reaches point
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;
e_rotation = w_rotation - theta; 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 // limit translation speed
if (u_translation > upper_thresh_translation) if (u_translation > upper_thresh_translation)
@@ -649,16 +624,14 @@ void CKobuki::goStraight(long double distance){
this->setTranslationSpeed(0); 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) { void CKobuki::doRotation(long double th) {
long double u = 0; // riadena velicina, uhlova rychlost robota pri pohybe long double u = 0; // riadena velicina, uhlova rychlost robota pri pohybe
long double w = th; // pozadovana hodnota v radianoch long double w = th; // pozadovana hodnota v radianoch
long double Kp = PI; long double Kp = PI;
long double e = 0; long double e = 0;
int thresh = PI/2; int thresh = PI / 2;
theta = 0; theta = 0;
x = 0; x = 0;
@@ -670,28 +643,31 @@ void CKobuki::doRotation(long double th) {
if (w > 0) { if (w > 0) {
while (gyroTheta < w) { while (gyroTheta < w) {
e = w - gyroTheta; e = w - gyroTheta;
u = Kp*e; u = Kp * e;
if (u > thresh) u = thresh; if (u > thresh)
if (u < 0.4) u = 0.4; u = thresh;
if (u < 0.4)
u = 0.4;
if (i < u) { if (i < u) {
u = i; u = i;
} }
std::cout << "Angle: " << gyroTheta << " required:" << w << std::endl; std::cout << "Angle: " << gyroTheta << " required:" << w << std::endl;
this->setRotationSpeed(-1*u); this->setRotationSpeed(-1 * u);
usleep(25*1000); usleep(25 * 1000);
i = i + 0.1; i = i + 0.1;
} }
} } else {
else {
while (gyroTheta > w) { while (gyroTheta > w) {
e = w - gyroTheta; e = w - gyroTheta;
u = Kp*e*-1; u = Kp * e * -1;
if (u > thresh) u = thresh; if (u > thresh)
if (u < 0.4) u = 0.4; u = thresh;
if (u < 0.4)
u = 0.4;
if (i < u) { if (i < u) {
u = i; u = i;
@@ -699,7 +675,7 @@ void CKobuki::doRotation(long double th) {
std::cout << "Angle: " << gyroTheta << " required:" << w << std::endl; std::cout << "Angle: " << gyroTheta << " required:" << w << std::endl;
this->setRotationSpeed(u); this->setRotationSpeed(u);
usleep(25*1000); usleep(25 * 1000);
i = i + 0.1; i = i + 0.1;
} }
} }
@@ -709,18 +685,17 @@ void CKobuki::doRotation(long double th) {
usleep(25*1000); usleep(25*1000);
} }
// kombinuje navadzanie na suradnicu a rotaciu o uhol, realizuje presun na
// kombinuje navadzanie na suradnicu a rotaciu o uhol, realizuje presun na zvolenu suradnicu // zvolenu suradnicu v suradnicovom systeme robota
// v suradnicovom systeme robota
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;
th = atan2(yy,xx); th = atan2(yy, xx);
doRotation(th); 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 // resetnem suradnicovu sustavu robota
x = 0; x = 0;
@@ -728,11 +703,10 @@ void CKobuki::goToXy(long double xx, long double yy) {
iterationCount = 0; iterationCount = 0;
theta = 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); usleep(25 * 1000);
return; return;
} }