Date: 11.01.2008
Duration of activity: 5 hours
List of participants: Alessandro, Daniela, Samuele
Goal: Make the robot able to describe, in a two dimensional system, the location of the party where it is "dancing".
Plan:
- figure out how to use the tachometer of the NXT motors;
- understand how TachoNavigator works;
- implement a specific Navigator to make the robot move "according to Braitenberg" and localize the resulting position;
- add a function to make the robot go in a specific position in the map.
- test the accuracy of our Navigator
The only tool we can use to determine the distance covered and the direction of the robot is the tachometer included in the NXT Motors. The leJOS API provides the interface Tachometer (implemented by MotorPort) to interact with it, containing these methods:
- getTachoCount() - returns the rotation count of the wheel connected with the motor in degrees [0-360[
- resetTachoCount() - resets the tachometer count to 0;
The central idea of this lab session is to merge the concepts of "navigation" and "Braitenberg movement": using the Navigator API provided by leJOS the only way to make the robot move is imposing the distance or the angle that the robot has to drive. On the contrary we need that the robot remembers its position after a movement imposed just setting the speeds of the motors and the driving time.
The prototype of our "BraitenbergNavigator" consists of the following methods:
public void driveArc(int speedL, int speedR);The first one will be the core of the class that we are going to implement: it asks the robot to move along an arc just imposing to it the speeds of the motors and to keep track of its position. This is the main difference with the leJOS TachoNavigator and the reason why we are going to rewrite it.
public void stop();
public float getX();
public float getY();
public float getAngle();
The second method interrupts the movement of the robot and updates its position.
The last three trivial methods return in every moment the position and the direction of the robot.
Implementation
The driveArc method sets the speed of the motors as requested from the caller and imposes them a forward movement. Since the speeds requested as input can be negative values and the function setSpeed doesn't accept any negative values, we just flip the sign of the speed before passing it to the motor and use the method backward() to impose the movement.
When the stop function is invoked, the motors are stopped and the position of the robot is updated.
Therefore the core of the new class is the function updatePosition(). The aim of this method is to calculate the coordinate of the arrival position of the robot after it covered a bend.
The values t1, t2 in the code and in the picture below are the distances covered by the two wheels calculated from the values returned by getTachoCount() multiplied by the known diameter of the wheels.
t1 = motorLeft.getTachoCount() * wheel_diameter / 2;
Because the distance beetween the wheel is also known, it's easy to calculate the angle and the radius of the circumference which the robot is driving on.
alpha = (t1 - t2) / wheel_distance;
r = (t1 + t2) / (2 * alpha);
Thanks to trigonometric functions we obtain the distance covered on the axes, relative to the starting position.
relative_y = r * Math.sin(alpha);
relative_x = r * (1 - Math.cos(alpha));
A rotation and a translation are sufficient to obtain the new position on the absolute coordinate system.
The next step is to add a method
public void goTo(float newX, float newY)that makes the robot move to a position indicated by the absolute coordinates newY, newX.
We tried to implement this function using the corresponding one provided by leJOS Navigator: instancing a "normal" Navigator in the constructor of "our" navigator, resetting its position every time we call our goTo method and calling its goTo method with the position updated.
An easy test,
ournavigator.drawArc(300, 500);, discloses some mistake on the code of the goTo method: the travelled distance is correct, but the car is driving in a wrong direction. It reveals that the coordinate systems of the two Navigators are not compatible. We think about two solutions which can resolve the problem:
Thread.sleep(2000);
ournavigator.stop();
ournavigator.goTo(0, 0);
- convert the coordinate system of the new navigator to those of the old one (it seems that the angle is rotated of 90°);
- rewrite enterly the code of our goTo without using the "old" Navigator.
definitiveUpdatePosition();where rotate and driveForward are written using our coordinate system, thus we resolved also the first problem.
rotate(angleTo(getX(), getY(), newX, newY) - getAngle());
float distance = distanceTo(getX(), getY(), newX, newY); // cm
int speed = 500;
driveForward(speed); // degrees per second
Thread.sleep((int) (1000 * cmToDegrees(distance) / speed)); // milliseconds
stop();
Files:
BraitenbergNavigator.java
Conclusion:
At the end of this session we have at our disposal a shiny navigator that can be used to move the robot indicating just the speeds of the motors and the travelling time.
Simple tests like that one described above proved that the error is <10 cm out of a distance of 200 cm: we think this accuracy will be sufficient for our purposes, because it's important that the robot goes in the proximity of the party and not exactly in the same position of the other robots.
Further development
Adapt the coordinate system to that one used in standard leJOS API.
References:
- leJOS API (particularly the section about Motors and Tachometer) - http://lejos.sourceforge.net/p_technologies/nxt/nxj/api/lejos/nxt/Tachometer.html
- Brian Bagnall, Maximum LEGO NXT - Building Robots with Java Brains (Chapter 12 - Localization)
No comments:
Post a Comment