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

translation driver and testing

Browse Source
This commit is contained in:
Sam Hos
2024-10-01 16:36:04 +02:00
parent 7f4376c2e5
commit 2e0a27c732
2 changed files with 11 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -589,21 +589,21 @@ long CKobuki::loop(void *user_data, TKobukiData &Kobuki_data) {
// povie kobukimu ze ma ist niekolko metrov dopredu alebo dozadu, rozhoduje znamienko // tells the kobuki to go a few meters forward or backward, the sign decides
// funkcia kompenzuje chodenie rovno pomocou regulatora, interne vyuziva setArcSpeed a // the function compensates for walking straight with the controller, internally it uses setArcSpeed and
// ako spatnu vazbu pouziva data z enkoderov // uses encoder data as feedback
void CKobuki::goStraight(long double distance){ void CKobuki::goStraight(long double distance){
long double u_translation = 0; // riadena velicina, rychlost robota pri pohybe long double u_translation = 0; // controlled magnitude, speed of the robot in motion
long double w_translation = distance; // pozadovana hodnota long double w_translation = distance; // requested value
// parametre regulatora // controller parameters
long double Kp_translation = 4500; long double Kp_translation = 4500;
long double e_translation = 0; long double e_translation = 0;
int upper_thresh_translation = 600; int upper_thresh_translation = 600;
int lower_thresh_translation = 40; int lower_thresh_translation = 40;
int translation_start_gain = 20; int translation_start_gain = 20;
long double u_rotation = 0; // riadena velicina long double u_rotation = 0; // controlled magnitude
long double w_rotation = 0; long double w_rotation = 0;
long double Kp_rotation = 57; long double Kp_rotation = 57;
long double e_rotation = 0; long double e_rotation = 0;
@@ -613,7 +613,7 @@ void CKobuki::goStraight(long double distance){
theta = 0; theta = 0;
long i = 5; 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; e_translation = w_translation - x;
u_translation = Kp_translation * e_translation; u_translation = Kp_translation * e_translation;
@@ -640,10 +640,9 @@ void CKobuki::goStraight(long double distance){
if (u_rotation == 0) { if (u_rotation == 0) {
u_rotation = -32767; u_rotation = -32767;
} }
//send command to robot
this->setArcSpeed(u_translation, u_rotation); this->setArcSpeed(u_translation, u_rotation);
usleep(25*1000);
// increment starting speed // increment starting speed
i = i + translation_start_gain; i = i + translation_start_gain;
} }
@@ -705,8 +704,7 @@ void CKobuki::doRotation(long double th) {
} }
} }
std::cout << "stop the fuck!" << std::endl; std::cout << "Rotation done" << std::endl;
// usleep(25*1000);
this->setRotationSpeed(0); this->setRotationSpeed(0);
usleep(25*1000); usleep(25*1000);
} }

2
src/C++/Driver/src/main.cpp
View File

@@ -36,7 +36,7 @@ int movement()
if (text == 1) if (text == 1)
{ {
robot.goStraight(10); robot.goStraight(1);
} }
else if (text == 2) else if (text == 2)
{ {