Positionssensor

Der Positionssensor, auch GPS genannt, gibt die Position des Roboters in Metern an. Beim Starter-Roboter heißt er gps. Du brauchst ihn, um Wandmarkierungen zu melden, und er hilft sehr dabei, systematisch zu erkunden.

getValues() gibt [x, y, z] zurück. X und Z sind horizontal, Y ist vertikal. Verwende also X und Z für die Position im Labyrinth:

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

Das GPS hat ein Rauschen von wenigen Millimetern, daher ändern sich die Werte leicht, auch wenn der Roboter stillsteht. Vergleiche Positionen nie auf exakte Gleichheit.

Beispiel: Entfernung vom Start

Dieses Programm fährt vorwärts, bis der Roboter eine halbe Kachel von seinem Startpunkt entfernt ist:

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