I have a piece of code which is showing some weird behaviour. The code works as expecred when it is kept out of the for loop but once I place it inside the for loop, it just keeps on repeating the same value once and again.
const double *north = cp->getValues();
angle1 = atan2(north[1], north[2]);
bearing = (angle1 - 1.5708) / M_PI * 180.0;
std::cout<<"Bearing inside : "<<bearing<<std::endl;
if (bearing < 0.0)
bearing = bearing + 360.0;
t = abs(bearing - turn);
This portion of code to supposed to update the *north everytime the object rotates. This happens whenever I keep this code outside my loop. But once the code is kept inside a for loop as shown below, the value of *north remains constant.
for(int i=0;i<100;i++)
{
double t_modifier = (180.0 - abs(t)) / 180.0;
double threshold = 2.0;
double Speedleft = 1.0;
double Speedright = 1.0;
int identifier;
if(int(t)<0){
Speedleft = -1;
}
else if(int(t)>0){
Speedright = -1;
}
leftSpeed = threshold * Speedleft;
rightSpeed = threshold * Speedright;
{
wheels[0]->setVelocity(leftSpeed);
wheels[1]->setVelocity(rightSpeed);
wheels[2]->setVelocity(leftSpeed);
wheels[3]->setVelocity(rightSpeed);
}
const double *north = cp->getValues();
angle1 = atan2(north[1], north[2]);
bearing = (angle1 - 1.5708) / M_PI * 180.0;
std::cout<<"Bearing inside : "<<bearing<<std::endl;
if (bearing < 0.0)
bearing = bearing + 360.0;
t = abs(bearing - turn);
}
You should call the step
method in the loop:
https://cyberbotics.com/doc/reference/robot#wb_robot_step
An in-depth explanation of the step
method:
https://cyberbotics.com/doc/guide/controller-programming#the-step-and-wb_robot_step-functions