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