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; } } }