Wavefront_Code.txt (31049Bytes)
This is my first Visual C# Program.
After a few attemps of trying to get wavefront mapping working on an Arduino, I decided to try simulate it on a PC. After playing around for the last day and a half it's started to evolving into a control program for a robot.
At the moment, I've got the simulation side working. How it works:
First, you can add a Robot to the image (everything is only 1 pixel) specifing a X & a Y Co-ordinate. The robots pixel is Blue.
A set of manual controls appear, where you can turn the robot right or left and move it forward (The Robot pixel moves and the Robots direction is displayed).
To simulate receiving data from a robot you can input an Angle (the Servos Angle) and the distance (IR/Ping) reading from a sensor. When you click "Map Reading" Button, it calculates the the X, Y Co-ordinate and places a Black (object) pixel on the map. It does take into account the robot's X/Y Location and the direction it's facing.
And at last you can add a goal (Red) pixel to the map.
It's not to clear, but the black outline is a simulated map (Save Map & Open Map work thankfully so testing is easier). The Lone pixel on the left is the Blue Robot, and on the right is the red goal (the Red Value is 255, the highest it can be). You can also now see the Robot Controls Panel (containing Movement buttons, Robot Direction and location) and the Goal Location panel.
Clicking the "Fill WaveFront" button finds the goal pixel and paints all the pixels next to it red (minus 1 Red Value, say 254), and this keeps looping, painting the next pixels red minus 1 from the red value until it hits the robot's blue pixel.
If the black wasn't enclosed it would fill the whole pictureBox. You can't tell the difference between the goal and surrounding pixels but it does fade to black. Now Clicking the "Solve Step" button does what it suggests, if finds the highest red value pixel around the robot and moves in that direction.
It Leaves a white trail of where it's been and updates the robot's location and direction as it moves. It can't pass through the black pixels as they are Objects/walls. And it does it with as little turns as possible (Less drift error's). The Image above took about 20 clicks (robot grid movements).
It Continues until it find the pixel with the highest red value, the goal.
Below is a better image of the maps and movement as they progress.
I still have to add the serial communication and there's still a lot of ideas to be put into this, but I thought I would share my progess and the code (so far) for anybody thats interested or can suggest anything else to add.
A few Credits have to go to www.thesocietyofrobots.com as I derived my wavefront from thier tutorial and also to a Vano Maisuradze for posting a Lock/Unlock bitmaps into memory code, it makes handling bitmaps 100 times faster.
The idea is... In the end anybody with a robot that can communicate to a PC through Serial (Bluetooth/XBee) will be able to download this and have a simple Mapping Solution.