void MyRobot::run()
{
while (step(_time_step) != -1)
{
//Get the encoder values
_left_encoder = getLeftEncoder();
_right_encoder = getRightEncoder();
//Converts rad to meters
if(flag == 2 || flag == 8){
_distance = _right_encoder/ENCODER_RESOLUTION*WHEEL_RADIO;
}else{
_distance = _left_encoder/ENCODER_RESOLUTION*WHEEL_RADIO;
}
print_odometry_data();
//switch case to make the necesary steps to avoid the obstacle
switch (flag){
case 1:
cout << "linea recta"<< endl;
goStraight(5);
break;
case 2:
turn_left();
break;
case 3:
goStraight(3.5);
break;
case 4:
turn_right();
break;
case 5:
goStraight( 7);
break;
case 6:
turn_right();
break;
case 7:
goStraight(3.5);
break;
case 8:
turn_left();
break;
case 9:
goStraight(5);
break;
default:
_left_speed = 0;
_right_speed = 0;
break;
}
setSpeed(_left_speed, _right_speed);
}
}
void MyRobot::run()
{
// Main loop:
// Perform simulation steps of 64 milliseconds
// and leave the loop when the simulation is over
while (step(_time_step) != -1)
{
//Get encoders values
_left_encoder = getLeftEncoder();
_right_encoder = getRightEncoder();
//Converts rad to meters
_distance = _left_encoder/ENCODER_RESOLUTION*WHEEL_RADIO;
print_odometry_data();
//Robot control
if(_left_encoder > _right_encoder)
{
_mode = TURN_LEFT;
}
else
{
_mode = TURN_RIGHT
}
setSpeed(_left_speed, _right_speed);
if (_distance > TOTAL_DISTANCE)
{
_left_speed = 0;
_right_speed = 0;
setSpeed(_left_speed, _right_speed);
}
switch (_mode){
case STOP:
_left_speed = 0;
_right_speed = 0;
break;
case FORWARD:
_left_speed = MAX_SPEED;
_right_speed = MAX_SPEED;
break;
case TURN_LEFT:
_left_speed = MAX_SPEED *0.9;
_right_speed = MAX_SPEED;
break;
case TURN_RIGHT:
_left_speed = MAX_SPEED;
_right_speed = MAX_SPEED *0.9;
break;
default:
break;
}
}
}
