Sensor de posición

El sensor de posición, o GPS, da la posición del robot en metros. En el robot inicial se llama gps. Lo necesitas para informar de las marcas en las paredes, y ayuda mucho a explorar de forma sistemática.

getValues() devuelve [x, y, z]. X y Z son horizontales e Y es vertical, así que usa X y Z para la posición en el laberinto:

Python

from controller import Robot

robot = Robot()
timeStep = int(robot.getBasicTimeStep())

gps = robot.getDevice("gps")
gps.enable(timeStep)

while robot.step(timeStep) != -1:
    x, y, z = gps.getValues()  # In meters. X and Z are horizontal, Y is vertical.
    print("x:", round(x, 3), "z:", round(z, 3))

C/C++

#include <iostream>
#include <webots/Robot.hpp>
#include <webots/GPS.hpp>

using namespace webots;
using namespace std;

int main() {
  Robot *robot = new Robot();
  int timeStep = (int)robot->getBasicTimeStep();

  GPS *gps = robot->getGPS("gps");
  gps->enable(timeStep);

  while (robot->step(timeStep) != -1) {
    const double *position = gps->getValues(); // In meters. X and Z are horizontal, Y is vertical.
    cout << "x: " << position[0] << " z: " << position[2] << endl;
  }

  delete robot;
  return 0;
}

El GPS tiene un ruido de unos pocos milímetros, así que los valores cambian ligeramente incluso cuando el robot está quieto. Nunca compares posiciones buscando una igualdad exacta.

Ejemplo: distancia desde la salida

Este programa avanza hasta que el robot está a media baldosa de donde empezó:

Python

from controller import Robot
import math

robot = Robot()
timeStep = int(robot.getBasicTimeStep())

gps = robot.getDevice("gps")
gps.enable(timeStep)
leftWheel = robot.getDevice("left wheel motor")
rightWheel = robot.getDevice("right wheel motor")
leftWheel.setPosition(float("inf"))
rightWheel.setPosition(float("inf"))

robot.step(timeStep)  # Step once, so that the GPS has a value
startX, _, startZ = gps.getValues()

leftWheel.setVelocity(3.0)
rightWheel.setVelocity(3.0)

while robot.step(timeStep) != -1:
    x, _, z = gps.getValues()
    distance = math.sqrt((x - startX) ** 2 + (z - startZ) ** 2)
    print("Distance from the start:", round(distance, 3), "m")
    if distance > 0.06:  # Half a tile
        leftWheel.setVelocity(0)
        rightWheel.setVelocity(0)
        break

C/C++

#include <iostream>
#include <cmath>
#include <webots/Robot.hpp>
#include <webots/GPS.hpp>
#include <webots/Motor.hpp>

using namespace webots;
using namespace std;

int main() {
  Robot *robot = new Robot();
  int timeStep = (int)robot->getBasicTimeStep();

  GPS *gps = robot->getGPS("gps");
  gps->enable(timeStep);
  Motor *leftWheel = robot->getMotor("left wheel motor");
  Motor *rightWheel = robot->getMotor("right wheel motor");
  leftWheel->setPosition(INFINITY);
  rightWheel->setPosition(INFINITY);

  robot->step(timeStep); // Step once, so that the GPS has a value
  double startX = gps->getValues()[0], startZ = gps->getValues()[2];

  leftWheel->setVelocity(3.0);
  rightWheel->setVelocity(3.0);

  while (robot->step(timeStep) != -1) {
    double x = gps->getValues()[0], z = gps->getValues()[2];
    double distance = sqrt(pow(x - startX, 2) + pow(z - startZ, 2));
    cout << "Distance from the start: " << distance << " m" << endl;
    if (distance > 0.06) { // Half a tile
      leftWheel->setVelocity(0);
      rightWheel->setVelocity(0);
      break;
    }
  }

  delete robot;
  return 0;
}