void MyRobot::run() { while (step(_time_step) != -1) { //Read the value of the encoders _left_encoder = getLeftEncoder(); _right_encoder = getRightEncoder(); //Convert the value of the left encoder to meters _distance = _left_encoder/60.61; if (_distance < desired_distance) { //If 1 wheel is faster than the other, we turn to go straight if(_left_encoder > _right_encoder) { _left_speed = MAX_SPEED-10; _right_speed = MAX_SPEED; } else { _left_speed = MAX_SPEED; _right_speed = MAX_SPEED-10; } } //If we are over the final line, we stop else { _left_speed = 0; _right_speed = 0; } setSpeed(_left_speed, _right_speed); } }
int MyRobot::move() { // Get encoders position double l_encoder = getLeftEncoder(); double r_encoder = getRightEncoder(); int mode = -1; // return variable, mode to robot movements. // If both sensors measure the same, the speed of both wheels is the same if(l_encoder == r_encoder){ mode = 0; } else{ // If left encoder measure is greater than right encoder measure // decrease left speed if(l_encoder > r_encoder){ mode = 1; } // Else, right encoder measure is greater than left encoder measure // decrease right speed else{ mode = 2; } } return mode; }
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() { double compass_angle; while (step(_time_step) != -1) { // Read the sensors const double *compass_val = _my_compass->getValues(); _encoder_right = getRightEncoder(); _encoder_left = getLeftEncoder(); // Convert compass bearing vector to angle, in degrees compass_angle = convert_bearing_to_degrees(compass_val); // Print sensor values to console cout << "Compass angle (degrees): " << compass_angle << endl; _posicion_final = _encoder_right/1000 *0.0825;// calculate the next position cout << "posicion :" << _posicion_final << endl; // control condition if(_posicion_final < 0.087){ if (compass_angle < (DESIRED_ANGLE - 2)) { // Turn right _left_speed = MAX_SPEED; _right_speed = MAX_SPEED - 15; } else { if (compass_angle > (DESIRED_ANGLE + 2)) { // Turn left _left_speed = MAX_SPEED - 15; _right_speed = MAX_SPEED; } else { // Move straight forward _left_speed = MAX_SPEED; _right_speed = MAX_SPEED; } } } else{ _left_speed = 0.0; _right_speed = 0.0; } // Set the motor speeds setSpeed(_left_speed, _right_speed); } }
MyRobot::MyRobot() : DifferentialWheels() { _time_step = 64; _left_speed = 0; _right_speed = 0; // Get and enable the compass device _my_compass = getCompass("compass"); _my_compass -> enable(_time_step); // Eneable and get encoders enableEncoders(_time_step); _left_encoder = getLeftEncoder(); _right_encoder = getRightEncoder(); }
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; } } }