AI and communication

While we still don't have anything new to show you right now, though that doesn't mean that we're slacking. We've started working on two different things that together should enable us to create a working prototype for our mapping robot.

I (Josef) am working on an AI for the robot so it can navigate through the room while scanning. I tried to create my own navigation class that would calculate the position of the robot based only on the rotation of the wheels, but for some reason it's not working. The algorithm to calculate the angle its facing seems to be working, but only certain values for some reason. Seems like a rounding error somewhere, but I don't know where.


public double getRobotangle(){
robotnewangle = robotangle - (Math.toRadians(CS.getDegreesCartesian()));
robotangle = (Math.toRadians(CS.getDegreesCartesian()));

leftwheelangle = ((Math.toRadians(Motor.A.getTachoCount())) - leftwheeloldangle);
rightwheelangle = ((Math.toRadians(Motor.B.getTachoCount())) - rightwheeloldangle);

leftdist = ((wheeldiameter*Math.PI)*leftwheelangle/(2*Math.PI));
rightdist = ((wheeldiameter*Math.PI)*rightwheelangle/(2*Math.PI));

return robotangle;
}


The algorithm to calculate it's position doesn't seem to work at all, even if I use values from the compass sensor. For some reason it never returns any data.


public Point getRobotpos() {
float x, y, hypotenuse;

hypotenuse = (float)(Math.sqrt((2*Math.pow((getRobotAverageDist()/robotnewangle),2))
-((2*Math.pow((getRobotAverageDist()/robotnewangle),2))*Math.cos(robotnewangle))));

x = (float)(robotpos.x + (Math.cos(getRobotAverageangle()*hypotenuse)));
y = (float)(robotpos.y + (Math.sin(getRobotAverageangle()*hypotenuse)));

robotpos.x = x;
robotpos.y = y;

return robotpos;
}


Feel free to check them both out on our Google Code site and come with tips if you have. Right now the code is a bit unstructured however. The algorithm to calculate the angle based on the rotation of the wheels is a comment right now in favor for a similar method that uses the compass sensor instead.

When my own class didn't work I was forced (for the time being anyway) to use the navigation class in leJOS, which unfortunately limits me to use only the methods provided by the class to navigate the robot. I'd rather be able to manipulate the motors at will, for example to be able to follow a wall easily.

I'm using behavior programming for my work, a really smart way of creating AI's, since it's so easy implementing, editing or removing different parts of it. I recommend reading the leJOS tutorial about it if you're interested.

Peter is working to improve our communication, which as you could see in our movie works already, but could be made a lot smoother and more structured. The idea of the new communication class is that you add data that you want to send to a queue. The class will then process one item at a time and send it over either USB or Bluetooth (depending on what you want to use). The receiver will add each received item to a list and you may then process them whenever you want. There is one client (NXT) part and one server (PC) part in this.

Hopefully, when we're done with these things, or at least got something that works, we can combine it with the scanning algorithm we already have and with small tweaks to the Graphic Interface a crude prototype that should be able to move around and scan a room, and in real-time paint it up on a computer screen.

/Josef

Going the Right Way

Now we've finally begun to build something that with a bit of imagination remotely resembles our ultimate goal. UltrasonicScanner is a stationary robot which is much like the UltrasonicTest, but now we've integrated communication between the computer and the NXT through Bluetooth and we can let the computer do all the calculation and show the results graphically.

UltrasonicScaner
http://code.google.com/p/penemunxt/source/browse/#svn/trunk/UltrasonicScannerClient/src
http://code.google.com/p/penemunxt/source/browse/#svn/trunk/UltrasonicScannerServer/src



The code is more or less CommunicationTest combined with UltrasonicTest. We've established streaming of both the ultrasonic sensor getDistance() and motor getTachoCount() (which returns the angle from it's original position) and then perform all the necessary calculations serverside. We also have a third "channel" streaming data to allow us to give commands in both directions. This means that both the NXT and the JAVA application have the ability to close both programs.

private Point getScreenPos(int Distance, int Angle) {
Angle += 90;
int x, y;
int distx, disty;

distx = (int) (((Math.min(ULTRASONIC_SENSOR_MAX_DISTANCE, Distance) / (double) ULTRASONIC_SENSOR_MAX_DISTANCE) * (getWidth() / 2)));
disty = (int) ((Math.min(ULTRASONIC_SENSOR_MAX_DISTANCE, Distance) / (double) ULTRASONIC_SENSOR_MAX_DISTANCE) * getHeight());
x = (int) ((distx * Math.cos((Angle) * Math.PI / 180)) + (getWidth() / 2));
x = getWidth() + (x * -1);
y = (int) (-1 * (disty * Math.sin(Angle * Math.PI / 180)) + getHeight());

return new Point(x, y);
}


This is the algorithm we use to calculate the coordinates on the screen based on the data from the NXT, and it's all straightforward mathematics.






As you can see on these pictures the "map" the robot managed to create isn't totally accurate. Apparently the sensor can't give accurate data when facing at an angle to a flat surface, with the result that flat surfaces seems to be curved around the robot. Once we have the robot mobile a lot of this problem should be solved.

If check out our videos you can also see a version before we implemented Bluetooth for it.

This is the final result:

It's working!

It's astonishing how much you can manage to do in just one day of coding. Or astonishing how long time coding takes, depending on how you look upon it. In one day we've started to familiarize ourselves with the NXT and we've created all kinds of smaller test programs, all of which you can find on our Google Code page.


HelloWorld
http://code.google.com/p/penemunxt/source/browse/#svn/trunk/HelloWorld/src

The first basic program to show that the NXT and leJOS works properly. The same program that every programmer has done at least once during his early career. What it does is to simply write a String on our NXT's screen. The leJOS code for writing on the screen of the NXT is:
LCD.drawString("String",x,y);

UltrasonicTest
http://code.google.com/p/penemunxt/source/browse/#svn/trunk/UltrasonicScannerClient/src
http://code.google.com/p/penemunxt/source/browse/#svn/trunk/UltrasonicScannerServer/src

In this project with its very intuitive name we started playing with the Ultrasonic Sensor. This works like a sonar; it sends out ultrasonic sound and measures the time to the echo. To use the sensor you simply declare the sensor, which then has a number of predetermined methods. Note that you have to specify the port he sensor is located in as well.
UltrasonicSensor sensor = new UltrasonicSensor(SensorPort.S1);
int distance = sensor.getDistance();
LCD.drawInt (distance , 1, 1);


These lines will cause the sensor to measure the distance ahead of it and write out the distance in centimeters on the display.

We used this to create a program that measures the distance in a 180 degrees radius and then write it out on the LCD screen. Here we also used a motor to rotate the ultrasonic sensor. The motors also have a set of predetermined methods, but you don't have to declare them.

class HeadRotator extends Thread {
@Override
public void run() {
Motor.A.setSpeed(40);
while (!this.isInterrupted()) {
Motor.A.rotateTo(-100, true);
Sound.beep();
while (Motor.A.getTachoCount() >= -90 && !this.isInterrupted()) {
}
try {
Thread.sleep(200);
} catch (InterruptedException e) {
}
Sound.beep();
Motor.A.rotateTo(100, true);
while (Motor.A.getTachoCount() <= 90 && !this.isInterrupted()) { } try { Thread.sleep(200); } catch (InterruptedException e) { } Sound.beep(); } } }

This loop makes the sensor rotate back and forth in a 180 degree arc. In a separate thread we continuously gather data which we then translate into coordinates on the LCD and write it out.

We noted that the sensor isn't very accurate when facing at an angle to a flat surfaces, and when the sensor sweeped over a flat surface the surface would appear curved on the screen. This is something we'll have to take into consideration later on. Also, we noticed the limits of the NXT' computing capacity, as it didn't quite manage to keep up with the data income. Though the most difficult part for it was to manage to write all the data to the LCD fast enough, so the screen flickered.


CommunicationTest
http://code.google.com/p/penemunxt/source/browse/#svn/trunk/CommunicationTestClient/src
http://code.google.com/p/penemunxt/source/browse/#svn/trunk/CommunicationTestServer/src

To solve the problem with the NXT's limited computing capacity we wanted to be able to run most of the computation on a server, in this case a computer. Also we want to be able to show the results on a real screen instead of the very limited LCD of the NXT. Thus, we had to establish a communication link.

The first comm test was just a simple app sending the data from a touch sensor to the computer through USB. When the touchsensor was pressed the computer app should exit. leJOS included some really great examples for rthis so it wasn't that hard to establish the first connection.

We extended the data transfer so that it also transmitted the data from the soundsensor aswell as the ultrasonicsensor in real time. The PC recieved the data and painted it to an applet as a graph. The PC also always sends back a status code telling the NXT if the status is OK or not, if it's not OK everything should shut down.

Then it wasn't that hard to switch to Bluetooth mode instead. The use the same connector but with different protocols:

USB:NXTConnector conn = new NXTConnector();
conn.connectTo("usb://")

Bluetooth:NXTConnector conn = new NXTConnector();
conn.connectTo("btspp://")

Though there are better ways to implement it which we will test later on. Anyhow we missed one important thing in the beginning, we forgot to add the reference to the BT package bluecove.jar. It took us at least one hour to figure out that this was the missing part.

Video of the CommunicationTest:


Summary
So what we've learned so far will be really important. Now we will use our know knowledge in BT communication and ultrasonic data to build a simple bot that rotates, collects US data and sends it to the computer to display it.

Stay tuned!!
/Peter And Josef

Links
http://lejos.sourceforge.net/nxt/nxj/api/index.html leJOS NXT API
http://lejos.sourceforge.net/nxt/pc/api/index.html leJOS PC API
http://lejos.sourceforge.net/nxt/nxj/tutorial/Communications/Communications.htm leJOS Communication Tutorial