Hello guys,
I need some help with a pathfinding robot using any kind of pathfinding whether it be wafrount or SLAM. I really eventually want to do a SLAM program, but I just want to get a feel for a code. I made a code, but I do not think it is working the way I want it to. I am using an Arduino UNO, but I have a Mega with me too, and I also have some SD cards if needed to store some of the grid. My robot consists of a servo connected to a Ping))) Ultrasonic range finder, a metal plate, two encoders, two QTI sensors, a speed controller/motor shield, Li-Po Battery, and two motors. I know how to use my speed controller, I made a few programs with it, It works great with Arduino. Thank you so much, Simon
I made a code, but I do not think it is working the way I want it to.
Feel free to expand on that.
int propagate_wavefront()
{
//clear old wavefront
unpropagate();
counter=0;//reset the counter for each run!
//Serial.print("hi_5");
while(counter<50)//allows for recycling until robot is found
{
x=0;
y=0;
while(x<GridSize && y<GridSize)//while the Map hasnt been fully scanned
{
//if this location is a wall or the goal, just ignore it
if (Map[x][y] != wall && Map[x][y] != goal)
{
//a full trail to the robot has been located, finished!
if (min_surrounding_node_value(x, y) < reset_min && Map[x][y]==robot)
{
//finshed! tell robot to start moving down path
return min_node_location;
}
//record a value in to this node
else if (minimum_node!=reset_min)//if this isnt here, 'nothing' will go in the location
Map[x][y]=minimum_node+1;
}
//go to next node and/or row
y++;
if (y==GridSize && x!=GridSize)
{
x++;
y=0;
}
}
counter++;
}
return 0;
}
void unpropagate()//clears old path to determine new path
{
//stay within boundary
for(x=0; x<GridSize; x++)
for(y=0; y<GridSize; y++)
if (Map[x][y] != wall && Map[x][y] != goal) //if this location is something, just ignore it
Map[x][y] = nothing;//clear that space
//store robot location in Map
Map[robot_x][robot_y]=robot;
//store robot location in Map
Map[goal_x][goal_y]=goal;
}
//if no solution is found, delete all walls from Map
void clear_Map(void)
{
for(x=0;x<GridSize;x++)
for(y=0;y<GridSize;y++)
if (Map[x][y] != robot && Map[x][y] != goal)
Map[x][y]=nothing;
}
//this function looks at a node and returns the lowest value around that node
//1 is up, 2 is right, 3 is down, and 4 is left (clockwise)
int min_surrounding_node_value(int x, int y)
{
minimum_node=reset_min;//reset minimum
//down
if(x <(GridSize-1))//not out of boundary
if (Map[x+1][y] < minimum_node && Map[x+1][y] != nothing)//find the lowest number node, and exclude empty nodes (0's)
{
minimum_node = Map[x+1][y];
min_node_location=3;
}
//up
if(x > 0)
if (Map[x-1][y] < minimum_node && Map[x-1][y] != nothing)
{
minimum_node = Map[x-1][y];
min_node_location=1;
}
//right
if(y < (GridSize-1))
if (Map[x][y+1] < minimum_node && Map[x][y+1] != nothing)
{
minimum_node = Map[x][y+1];
min_node_location=2;
}
//left
if(y > 0)
if (Map[x][y-1] < minimum_node && Map[x][y-1] != nothing)
{
minimum_node = Map[x][y-1];
min_node_location=4;
}
return minimum_node;
}
//UPDATE Map BY SCANNING FOR WALLS
void find_walls(void)
{
//do not scan outside the border region
if((old_state==1 && robot_x==0) || (old_state==2 && robot_y==9) || (old_state==3 && robot_x==9) || (old_state==4 && robot_y==0))
return;
else
{
scan_angle=0;
//reset scanner
myservo.write(scan_angle);
delay(40);
object_found=0;
//do a full scan until something is sensed in the way
while(scan_angle <90 && object_found==0)
{
float volts;
float distance2 = 0;
float Wdistance2 = 0;
float distance3 = 0;
float Wdistance3 = 0;
for (int index = 0; index<numReadings;index++)
{ // take x number of readings from the sensor and average them
distance2 += (1000- analogRead(A1));
distance3 += (1000- analogRead(A4));
delay(10);
}
distance2 /=numReadings;
distance3 /=numReadings;
Wdistance3 = abs(distance3 *cos((scan_angle+90)*DegToRad));
Wdistance2 = abs(distance2 *cos(scan_angle*DegToRad));
/*
Serial.print(Wdistance2);
Serial.print(" ");
Serial.print(RobotWidth);
Serial.print(" ");
Serial.print(distance2);
Serial.print(" ");
Serial.println(scan_one_cell);
*/
if ((distance2 < scan_one_cell && Wdistance2 < RobotWidth )||(distance3 < scan_one_cell && Wdistance3 < RobotWidth ))//object found: add it to the Map
{
//account for angle of the robot wrt the Map, and stay in boundaries
if (old_state==1 && robot_x>0)
{
Map[robot_x-1][robot_y]=wall;
}
else if (old_state==2 && robot_y<(GridSize-1))
{
Map[robot_x][robot_y+1]=wall;
}
else if (old_state==3 && robot_x<(GridSize-1))
{
Map[robot_x+1][robot_y]=wall;
}
else if (old_state==4 && robot_y>0)
{
Map[robot_x][robot_y-1]=wall;
}
object_found=1;
}
//servo scan code
//-90 degrees at 300, lower goes CCW, 750 is centered, 1200 is far CW
myservo.write(scan_angle);
delay(12);
scan_angle++;
}
myservo.write(20);
if(object_found==0) //no object found, so clear it in the Map
{
//account for angle of the robot wrt the Map, and stay in boundaries
if (old_state==1 && robot_x>0)
Map[robot_x-1][robot_y]=nothing;
else if (old_state==2 && robot_y<(GridSize-1))
Map[robot_x][robot_y+1]=nothing;
else if (old_state==3 && robot_x<(GridSize-1))
Map[robot_x+1][robot_y]=nothing;
else if (old_state==4 && robot_y>0)
Map[robot_x][robot_y-1]=nothing;
}
}
//makes sure robot/goal location isnt replaced with a wall
//store robot location in Map
Map[robot_x][robot_y]=robot;
//store robot location in Map
Map[goal_x][goal_y]=goal;
}
Did you, by chance, read the sticky at the top of this section of the forum?
I'm guessing not.
Ok, I just really need help with creating an algorithm for wavefront or SLAM technique. I have a Ping))) a digital servo from specktrum (180 degrees) I also have two Fingertech robotics 50:1 gear ratio motors, I have a sabertooth 2x12 speed controller, I also have an arduino UNO hooked up right now and the pin connections right now are, 9>left motor 10>right motor 11>servo to turn the Ping))). I have two parallax QTI sensors mounted on the bottom so that if it comes across an edge of lets say a table, it doesn't fall off. I am willing to start my code over because I have had to do that many times before because the errors are gone. I also have two encoders, I don't know what type brand or anything about them other than they are the same and have four numberish things on them: 30, 0, B, 103. Thank-you for the help, Simon
No, you are absolutely right I din't read the sticky (all the way). Thank-you Simon (PS above is more specific I think)
Read section 6 of the sticky again, please, and see if that allows us to make sense of your code.
ok
Repost the code?
done
Well, I put on the encoders and am working on a point to point graphing system with the robot, to at least see if it works right. 
Ok I made a grid it turns and all and goes from point A to point B in lines.
#include <Servo.h>
Servo left;
Servo right;
int sensorPin = A0; // select the input pin for the potentiometer
int sensorPin2 = A1;
int ledPin = 13; // select the pin for the LED
int sensorValue = 0; // variable to store the value coming from the sensor
int k = 0;
int number = 1;
const int unit = 12000;
int leftservo = 0;
int rightservo = 0;
int x = 2;
int y = 3;
void setup() {
left.attach(9);
right.attach(10);
pinMode(ledPin, OUTPUT);
// Serial.begin(9600);
}
void loop() {
if (number >= 1 && number <= y) {
left.writeMicroseconds(1200);
right.writeMicroseconds(1200);
}
if (number >= y+1 && number <= y+1) {
left.write(120);
right.writeMicroseconds(1200);
// delay(1000);
//leftservo = 0;
//rightservo = 0;
//number++;
}
if (number >= y+2 && number <= y+2+x) {
left.writeMicroseconds(1200);
right.writeMicroseconds(1200);
}
sensorValue = analogRead(sensorPin);
k = analogRead(sensorPin2);
if (sensorValue >= 370) {
rightservo++;
if (rightservo >= (unit+10) && number != y+1) {
right.write(90);
number++;
delay(50);
rightservo = 0;
}
else if (rightservo >= (unit+10)/4 && number == y+1) {
right.write(90);
number++;
delay(50);
rightservo = 0;
}
}
if (k >= 171 && k < 200 && number != y+1) {
leftservo++;
if (leftservo >= unit) {
left.write(90);
delay(50);
leftservo = 0;
}
else if (leftservo >= unit/4 && number == y+1) {
left.write(90);
delay(50);
leftservo = 0;
}
}
else {
if (k >= 350) {
leftservo++;
if (leftservo >= unit && number != y+1) {
left.write(90);
delay(50);
leftservo = 0;
}
else if (leftservo >= unit/4 && number == y+1) {
left.write(90);
delay(50);
leftservo = 0;
}
}
}
}
Can anybody please help me out with an Ai algorithm? Please, Thanks Simon
Can anybody please help me out with an Ai algorithm?
To do what?
I would like for the robot to do at least basic maze solving, and be able to remember the path to get back to the start. I first started with a grid so that I can get it to at least be able to get from point A to point B. Thanks Simon
The manual for Pololu's m3pi rover includes code for line follower/maze solving. That code won't just compile on the Arduino, and it also won't just work on a custom built rover, but it's a perfectly valid source for understanding some of the easier methods of line following and maze solving.
I love your there are only 2 people who are in this world ones who know binary and ones who don't. Thanks for the response and I'll take a look, its just I kinda need a reference in Arduino because it would help me greatly understand this better. Maze solving as I can see is tricky, but I am not really for the "left turn" algorithm. Thanks Simon. PS: I'm sorry I am a bit tough but I need this to show my buddies when I get to high-school next year Thanks again.
Ugh this is so difficult
Please help?
What you are attempting to do is very, very difficult. SLAM is far from a "solved problem". For your small robot, you have a variety of tasks to perform, but it is difficult to say what they are because you haven't really described what your "maze" is going to look like. I have to assume that since you are using an ultrasonic sensor, that you are wanting to have a real maze with walls, correct? Have you built this maze yet?
You need to learn how to get your robot localize itself within that environment. By that I mean, from the robot's experience within the environment, it should be able to take sensor readings and then use that to determine where it is positioned at that moment in time within the environment. Once it knows that (and knows the destination of the goal), you then need to do path planning. Depending on the environment (whether it is a regular grid or not), this path planning can take the form of a discrete algorithm (like the wavefront algo or A*), or it might have to use a continuous algorithm of some sort.
Your robot will also need to be able to sense and understand how far it has gone from one point to another (wheel encoders and the like), along with what direction it is facing. Also note that since such sensors and methods are far from perfect, you are going to have some error in your sensor readings; with that in mind, you need to be able to adjust for this error, and use it to factor in a probabilistic manner your algorithms so that your robot has a sense of not only where it is positioned and how it is oriented in the maze, but to also what degree it believes that it is correct.
I am not an expert on this - far from it. I am merely telling you what I know from what I have read online, as well as what I have learned in the past 4 weeks or so taking the Udacity CS373 class:
http://www.udacity.com/overview/Course/cs373/CourseRev/apr2012
This is -not- an easy class, but it will give you more than enough info to get you on your way with your project, though I am not sure whether you have the mathematical knowledge yet to be able to understand some of the material (exposure to linear algebra and probability/statistics is helpful).