mirror of
https://gitlab.fdmci.hva.nl/technische-informatica-sm3/ti-projectten/rooziinuubii79.git
synced 2025-08-03 20:04:58 +00:00
added curly brackets
This commit is contained in:
@@ -4,19 +4,16 @@
|
||||
#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;
|
||||
}
|
||||
@@ -44,20 +41,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;
|
||||
}
|
||||
@@ -69,17 +63,13 @@ 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
|
||||
{
|
||||
} else {
|
||||
set_interface_attribs2(HCom, B115200,
|
||||
0); // set speed to 115,200 bps, 8n1 (no parity)
|
||||
set_blocking2(HCom, 0); // set no blocking
|
||||
@@ -103,37 +93,31 @@ int CKobuki::connect(char *comportT)
|
||||
}
|
||||
}
|
||||
|
||||
unsigned char *CKobuki::readKobukiMessage()
|
||||
{
|
||||
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
|
||||
{
|
||||
do {
|
||||
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
|
||||
{
|
||||
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)
|
||||
{
|
||||
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 =
|
||||
@@ -141,8 +125,7 @@ unsigned char *CKobuki::readKobukiMessage()
|
||||
outputBuffer[0] = buffer[0];
|
||||
int pct = 0;
|
||||
|
||||
do
|
||||
{
|
||||
do {
|
||||
Pocet = 0;
|
||||
int readpoc = (readLenght + 1 - pct);
|
||||
Pocet = read(HCom, outputBuffer + 1 + pct, readpoc);
|
||||
@@ -164,9 +147,15 @@ unsigned char *CKobuki::readKobukiMessage()
|
||||
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};
|
||||
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);
|
||||
@@ -174,18 +163,15 @@ void CKobuki::setLed(int led1, int led2)
|
||||
|
||||
// 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)
|
||||
{
|
||||
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)
|
||||
{
|
||||
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};
|
||||
@@ -197,8 +183,7 @@ void CKobuki::setTranslationSpeed(int mmpersec)
|
||||
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,
|
||||
@@ -222,10 +207,8 @@ void CKobuki::setRotationSpeed(double radpersec)
|
||||
pocet = write(HCom, &message, 14);
|
||||
}
|
||||
|
||||
void CKobuki::setArcSpeed(int mmpersec, int radius)
|
||||
{
|
||||
if (radius == 0)
|
||||
{
|
||||
void CKobuki::setArcSpeed(int mmpersec, int radius) {
|
||||
if (radius == 0) {
|
||||
setTranslationSpeed(mmpersec);
|
||||
return;
|
||||
}
|
||||
@@ -253,8 +236,7 @@ void CKobuki::setArcSpeed(int mmpersec, int radius)
|
||||
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,
|
||||
@@ -266,26 +248,24 @@ void CKobuki::setSound(int noteinHz, int duration)
|
||||
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);
|
||||
if (pthread_result != 0) {
|
||||
std::cerr << "Error creating thread: " << pthread_result << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
@@ -293,8 +273,7 @@ int CKobuki::measure()
|
||||
|
||||
// maximalne moze trvat callback funkcia 20 ms, ak by trvala viac, nestihame
|
||||
// citat
|
||||
if (ok == 0)
|
||||
{
|
||||
if (ok == 0) {
|
||||
loop(userData, parser.data);
|
||||
}
|
||||
free(message);
|
||||
@@ -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,25 +474,19 @@ 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)
|
||||
{
|
||||
if (GyroAngle < 0) {
|
||||
rad = GyroAngle + 360;
|
||||
}
|
||||
else
|
||||
{
|
||||
} else {
|
||||
rad = GyroAngle;
|
||||
}
|
||||
return (long double)rad * PI / 180.0;
|
||||
}
|
||||
|
||||
long CKobuki::loop(void *user_data, TKobukiData &Kobuki_data)
|
||||
{
|
||||
if (iterationCount == 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;
|
||||
@@ -526,35 +495,27 @@ long CKobuki::loop(void *user_data, TKobukiData &Kobuki_data)
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
else
|
||||
{
|
||||
} else {
|
||||
dLeft = Kobuki_data.EncoderLeft - prevLeftEncoder;
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
else
|
||||
{
|
||||
} else {
|
||||
dRight = Kobuki_data.EncoderRight - prevRightEncoder;
|
||||
}
|
||||
|
||||
long double dGyroTheta = prevGyroTheta - gyroToRad(Kobuki_data.GyroAngle);
|
||||
|
||||
if (dGyroTheta > PI)
|
||||
{
|
||||
if (dGyroTheta > PI) {
|
||||
dGyroTheta -= 2 * PI;
|
||||
}
|
||||
if (dGyroTheta < -1 * PI)
|
||||
{
|
||||
if (dGyroTheta < -1 * PI) {
|
||||
dGyroTheta += 2 * PI;
|
||||
}
|
||||
|
||||
@@ -565,12 +526,9 @@ long CKobuki::loop(void *user_data, TKobukiData &Kobuki_data)
|
||||
long double mLeft = dLeft * tickToMeter;
|
||||
long double mRight = dRight * tickToMeter;
|
||||
|
||||
if (mLeft == mRight)
|
||||
{
|
||||
if (mLeft == mRight) {
|
||||
x = x + mRight;
|
||||
}
|
||||
else
|
||||
{
|
||||
} else {
|
||||
x = x + (b * (mRight + mLeft)) / (2 * (mRight - mLeft)) *
|
||||
(sin((mRight - mLeft) / b + theta) - sin(theta));
|
||||
y = y + (b * (mRight + mLeft)) / (2 * (mRight - mLeft)) *
|
||||
@@ -619,14 +577,12 @@ long CKobuki::loop(void *user_data, TKobukiData &Kobuki_data)
|
||||
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
|
||||
// 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 u_translation =
|
||||
0; // controlled magnitude, speed of the robot in motion
|
||||
long double w_translation = distance; // requested value
|
||||
|
||||
// controller parameters
|
||||
@@ -647,8 +603,7 @@ void CKobuki::goStraight(long double distance){
|
||||
|
||||
long i = 5;
|
||||
// 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;
|
||||
u_translation = Kp_translation * e_translation;
|
||||
|
||||
@@ -663,18 +618,15 @@ void CKobuki::goStraight(long double distance){
|
||||
u_translation = lower_thresh_translation;
|
||||
|
||||
// rewrite starting speed with line
|
||||
if (i < u_translation)
|
||||
{
|
||||
if (i < u_translation) {
|
||||
u_translation = i;
|
||||
}
|
||||
|
||||
if (fabs(u_rotation) > 32767)
|
||||
{
|
||||
if (fabs(u_rotation) > 32767) {
|
||||
u_rotation = -32767;
|
||||
}
|
||||
|
||||
if (u_rotation == 0)
|
||||
{
|
||||
if (u_rotation == 0) {
|
||||
u_rotation = -32767;
|
||||
}
|
||||
// send command to robot
|
||||
@@ -688,9 +640,9 @@ void CKobuki::goStraight(long double distance){
|
||||
|
||||
/// 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
|
||||
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;
|
||||
@@ -703,20 +655,18 @@ void CKobuki::doRotation(long double th)
|
||||
|
||||
long double i = 0;
|
||||
|
||||
if (w > 0)
|
||||
{
|
||||
while (gyroTheta < w)
|
||||
{
|
||||
if (w > 0) {
|
||||
while (gyroTheta < w) {
|
||||
e = w - gyroTheta;
|
||||
u = Kp * e;
|
||||
|
||||
if (u > thresh)
|
||||
if (u > thresh) {
|
||||
u = thresh;
|
||||
if (u < 0.4)
|
||||
}
|
||||
if (u < 0.4) {
|
||||
u = 0.4;
|
||||
|
||||
if (i < u)
|
||||
{
|
||||
}
|
||||
if (i < u) {
|
||||
u = i;
|
||||
}
|
||||
|
||||
@@ -725,11 +675,8 @@ void CKobuki::doRotation(long double th)
|
||||
usleep(25 * 1000);
|
||||
i = i + 0.1;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
while (gyroTheta > w)
|
||||
{
|
||||
} else {
|
||||
while (gyroTheta > w) {
|
||||
e = w - gyroTheta;
|
||||
u = Kp * e * -1;
|
||||
|
||||
@@ -738,8 +685,7 @@ void CKobuki::doRotation(long double th)
|
||||
if (u < 0.4)
|
||||
u = 0.4;
|
||||
|
||||
if (i < u)
|
||||
{
|
||||
if (i < u) {
|
||||
u = i;
|
||||
}
|
||||
|
||||
@@ -755,10 +701,9 @@ void CKobuki::doRotation(long double th)
|
||||
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)
|
||||
{
|
||||
// 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;
|
||||
|
Reference in New Issue
Block a user