Artificial Intelligence: Sokoban Game
Sokoban is a classic game invented in Japan. The original version of the game was written by Hiroyuki Imabayashi, winning it with a programming contest in 1980.
Sokoban term means “store supervisor” (in English “warehouse keeper”) and it is one of the most loved games of logic, specific levels being created today.
The aim of the Sokoban game is carrying boxes on the target area by pushing them. The user controls the movement of Sokoban. He can move up, down, left and right cannot pass through walls. Can only push a box at the same time (cannot shoot).
To solve the game that all boxes must be carried on the target areas. Seems simple but in some small maze very difficult problems can arise. Finding a solution might can take hours or days.
Sokoban has some significant properties. For example, pushing boxes to the target areas does not mean that we have a solution. In some cases, movements unintuitive be made for reaching a solution. Order to push boxes is always important. Sometimes the removal of boxes from a target area is further, though at first push blocks on the target areas is quite simple, the last block or even Sokoban could not be held to complete the game.
Sokoban problem:
We had to find a path to resolve the Sokoban game and to test the path with the robot we made.
First we are going to speak about the features of the robot and next about the algorithm to move the robot and after that we will try to find a solution of this problem using the AI knowledge.
Description of the robot
Requirements:
The robot has to solve a Sokoban game. This game is represented by the black line on the white floor and some can which represent diamonds. In order to resolve the Sokoban, the robot has to:
-Moving on the map
-Push diamonds
-Read its position
1 Moving on the map:
In order to give to the robot the ability to move, two of Lego servo motors been used, one for each side.
Each motor has a built-in rotation sensor. This lets to control the robot’s movements precisely. The rotation sensor measures motor rotations in degrees or full rotations [accuracy of +/- one degree]. One rotation is equal to 360 degrees, so if its set a motor to turn 180 degrees, its output shaft will make half a turn.
The idea was to make a four wheels drive but could not use more than two motors. So its been decided to use chain to transmit the power from front wheels to back wheels. You can see this on the picture:
2 Push diamonds:
The main goal of the robot is to push the diamond in the good place. The robot can just push the diamonds, it can’t hold or turn with them. It just needed something to keep the diamonds in front of the robot. So we just added some Lego components in front of the robot.
3 Read its position:
The robot has to know where is it on the map, to detect the line, to follow it, to detect intersections. To do this, it needs a light sensor like this below. It enables the robot to distinguish between light and dark, as well as determine the light intensity in a room or the light intensity of different colors. But its been just used to determine the difference between a dark line and the white floor. To follow a line, its been decided to put three sensors side by side, in a one row.
The middle sensor is to check if the robot is following the dark line. The robot used the two sensors in the extremities to reposition the robot when it goes too much on the left or on the right. If the middle sensor doesn’t see the black line and one of the other see it, the robot turn on the right side in order to go back to the right position.
Moreover, the left and right sensors been used to detect an intersection. Indeed, when the middle sensor detects a black line and the two others too, it’s an intersection on the map.
In order to control all of this, the robot needs a “brain”. That’s why its been used NXT Intelligent Brick.
The NXT is a 32-bit microprocessor with a flash memory. Its been programmed with the NXC language (Not eXactly C) and the software” ROBOTC for Mindstorms “as an environment.
I can say that only the robot was very distinctive. We used a chain to move wheels. The construction of the robot was not heavy but strong and it was very easy to remove the white NXT brick. The point of the mass was in the center, between two motors and very close to the floor that is why the robot was very stable and could move faster.
Below is the final construction design of the robot.
Analyse of the construction:
Firstly, lets speak about advantages of the construction and next about drawbacks.
The first advantage of the robot is about motility. I built a 4 wheel drive with each side independent of the other. So it can turn one side in one way and the other side to the opposite in order to turn faster. To do this, there were two possibilities. The first is to make a tracked vehicle like a tank but this solution harms to the speed of the robot, so its been decided to use real tyres with chain to transmit the power to the wheels. The second advantage of this construction is to use a larger construction than a higher in order to improve the stability when turning. The third good point of the robot is the solidity, build a strong construction to avoid any problems.. To resume these advantages in one word, agility.
The main problem of the robot was when it pushed a can, It took the can little too high.
So it had the risk to strike down the can, the robot took a can and stroke it down.
Also, the surface was not plain because of black tape on the floor.
The second problem was with sensors that used three sensors side by side so that had a problem to stop the robot at the good position to leave the can. The three sensors been used for navigation.
To conclude, we built a good robot to navigate on the map but not for interact with diamonds. So if more there was more time available, we could improve all the part concerning the interaction with the can. We would probably change the way to push the can in order to continue with the three sensors side by side because it’s a really efficient solution.
Testing phase:
In order to improve the robot, we realised some simple test.
1. Solidity:
2. Value of sensors
3. Value of the speed for motors
Solidity:
We realised a simple test to prove the choc resistance, we let fall the structure of the robot (without the NXT block, because of the price) for a distance of 1 meter. The robot passed this test.
Value of sensors:
This value is the reason of a lot of problem of portability. Because the robot was working well with white floor and black line but not the same with dark line on gray floor. Firstly, the value was in 45% and decreased this value until to arrive to a good ratio between portability and performance with a value of 31.
Value of the speed of the motors:
When you design a robot, its all about a speed, so we had to play with the speed of motors. But the problem is, if the robot is too fast, it doesn’t see the line. So spent a lot of time to define the highest value for this speed with keeping a good understanding of black line.
Robot’s contribution:
With the legoNXT, we created a robot that is able to solve the Sokoban problem. That was interesting because we didn't work just with programming but did also all the construction of the robot.
Controls of the robot:
To resolve the Sokoban Game, the robot has to do some actions. A list of these actions is given to the robot by a string of characters. These characters and the correspondent actions are:
· “s” -> Go straight
· “b” -> Go back
· “l” -> Turn 90° on the left
· “r” -> Turn 90° on the right
· “f” -> Leave the diamond
The robot receives a string for the AI program which gives it all these actions. Here is an example of the string:
{‘l’,’s’,’r’,’s’,’s’,’r’,’s’,’s’,’s’,’s’,’s’,’s’,’b’,’b’,’f’};
Matrix idea:
In the meantime, we found a very useful idea that will try to shortly describe. If we had more time, we would use it but of course with some improvements.
Because we faced the first time with a problem of connecting the software with the hardware we did not know anything about an interface and how the robot should communicate with the second program. It could be a kind of string or a kind or matrix. We decided to use the second solution firstly but it was a wrong decision. We forgot about the old proverb: the simplest solutions are always the best…
So why for me this matrix was better to pass commands to the robot than a simple string?
We thought that it would be easier to implement a kind of one while loop statement and the robot would follow to the nearest number. But we did not know that without any collection it would be very hard to do.
Ideally, an average solution of the Sokoban game have nearly 160 but not more than 200 movements using the map similar to the size. The robot had to follow the way below:
So, lets put put some numbers that will show the way. All numbers will be increased after every movement. So the first brick will start with 0, the second one with 1, one after the second one has 2 and … like this pattern, it must just follow the pink color:
After each movement, we have decreased all numbers minus 1:
The situation is repeated every next movement. Of course, if I wanted to move back, we would just add plus 1 to all numbers. After every movement we just look for the closest one number, in this case it is 1 digit. We tried to develop this solution but met some problems. For example the first one was: what happened with the crossing one line:
What about this brick 1/15? We developed it creating each node for the one cell and each node has a collection of numbers. It was very time-wasting solution but it worked. We created almost all program with that trick. The problem started with passing it to this robot. That is why, its been decided to use just a simple string and the counter that pointed to the current character of the string. How to pass the collection to the robot? Unfortunately we could not use serialization or something like that. We could create collection but it was easier to use mentioned above string. That was the first problem we met. The algorithm was:
1. find solution (AI part, implemented manually)
2. define start point and the end point of solution
3. fill with numbers (AI part, done manually)
4. check if exists the nearest one
5. if not leave while
6. check if the nearest one is the last one
7. if yes leave while
8. if not move to the nearest one and update position and decrease numbers
Implementation:
Its been decided to work with one main task which read the string of characters and tell the action to do the robot. In order to do this, we added seven functions:
We had to define a value for the sensors in order to differentiate black line from white floor. It’s a value in percentage. Firstly, we put 45% because it worked with real white floor. But the problem was that the floor was not really white but grey. So, we decided to change this value to 31% and it worked better.
You can see all the code with comments at the end of this report.
Conclusion:
A lot of test been made to improve this code and components added to the robot in order to make it efficient enough. The problem we had was the functionality of the sensors because it was working on a white floor but when tried on the grey floor, there was some problem. So if there was more time available. we coould focus on improving this feature. We would also use precompiled instructions of C, like #DEFINE LIGHT_LEVEL 48
A*
Therefore, its been decided to use A* algorithm to move robot from point to point. Some parts of the algorithm been modified, mainly connected with displaying result.
Firstly, we used Manhattan type of defining the distance between a current point and the goal point, we changed it after to a2+b2=c2 later we used both with modification and after that left to Manhattan way. Why? Because the solution had the smallest amount of turnings.
This is the A* implemented algorithm:
Add start node to the list of open nodes
Repeat:
1 Find the highest f in the list of open nodes.
2 Current node = highest f node
3. Add Current node to the list of closed nodes
4 For all neighbors of current node we do:
a. If it is wall or something like that or it is in the closed list I ignore it
b. If neighbor is not at the open node list I add it. Current node will be the parent and calculate F, G and H
c. If neighbor has been already at the open list, it checks if the path leading to this node is shorter. This done by checking G only. Smaller G means that path is better. If it is shorter, we changed parent node to this current node and calculated again G and F. If open list had was sorted with criteria F, it had to be sorted it again.
5. Stop if:
a. If destination path is added to the close node list. It means that the path is found
b. If open list nodes is empty. Path was not found.
6. Save the path.
Sometimes words complicated the issue so if you look at the picture below:
As you can see this is the simply A* algorithm. There been made some changes, especially the way to find neighbors and some changes with displaying the path. Its been created a Windows Form Application. Here are some pictures, first of the interface, the second one of a* example:
Robot travels from point to point and tries to avoid bricks. We created a kind of progress bar to see the progress of the calculation. If we wanted to use solution only between points, We just had to check box “between points”. If we wanted to find the solution of all the Sokoban game, we had to click “All game” and after that “Solve”. The step button is to show the matrix after each movement which is used Manhattan Way Calculation. This calculation has the smallest amount of turnings but then we didn't think it is the best idea. We could use just a2+b2=c2 and improve heuristic function, for example it will return smaller value if the next step connected is with the turning.
First of all, we had to solve the problem with the can: How to turn with the can? The A* algorithm is been modified in order to do this. Action added to each node, the least direction of movement. If a previous one was different, a current started a new recursion with an A* algorithm. Points depended with direction of robot was passed to the new recursion, for example if it was north and it would be nice to turn left, start and end point are showed below.
We had two matrixes, one to solve the robot, the second one to path finding. Each cell in the matrix has some value, which is G.
We created a parser to save the string. We reached this point of Sokoban solving and we didn't know why a kind of bridge was created, we could pass it, but more about it below. You can see all of the description in these pictures:
If the robot could not push from one side, for example in this case from right it just marked the way with 0 with one matrix or with smaller numbers and the algorithm should start again. We marked zeros to not use the same way again.
Barrier and bridge
The barrier was found after implemented a* algorithm and started thinking about AI planner. The solution for this algorithm, described below.
AI planner
First of all, we had no idea how to solve it. We tried with the brutal force algorithm but with 1 GB RAM Fujitsu Siemens Amilo it was impossible. Later we found an idea which was not a really AI solution but it was a little bit closer, just better than brutal force. It means: find the nearest one and move to the shortest hole. It was not worked well, also so found something like that:
An AI algorithm with A*. Inside loop is to find best way from point to point, outside to define which points diamonds should be choose. Inside algorithm was described above let`s start talking about the outside A*.
Because it is AI subject, started to find a kind of algorithm that is well described and it is very easy but it has a kind heuristic function, a tree of the solution. The main problem was:
How should define each node in the tree solution, what a start point (root) is and what end point node is. That was a real problem. A* has also a tree, each node is the position of in the matrix. The start node is the robot position and the goal is a destination point. In a main while loop it checks using heuristic which node should be chosen and which can be chosen. The node in outside X algorithm should be similar to A* node. The next problem was that we could not define node after 5 or 6 movements because we could lost five or six movements that could be necessary to solve a kind of board. It had to push some diamond to the position totally unconnected with their destination points but without that solving a map would be impossible. If it pushed only diamonds to holes, we could not solve the problem. That is why we changed the idea to use only one AI algorithm.
Assumption data:
· Node was the matrix state,
· Root was the matrix with start robot position,
· Destination root was the matrix position with all diamonds in their holes.
· Heuristic function:
d-distance between diamonds to their holes in the next movement.
c-If diamonds are moved to their holes in the next movement add “c”.
m-distance between robot to the first diamond.
F = 0.6d + 0.1c + 0.3m.
· Try to choose the lowest F
· The algorithm is similar to the mentioned before A*, it has open list, closed list…
· The algorithm creates a kind of solution tree
· Tree is physically implemented as a linked list or tree collection in .NET 3.5
· Node is added after every movement to the next brick
· If algorithm cannot add node it goes one node up and the checks if it is possible, just like in a normal A* (open and close node list)
Firstly, its been checked the neighbors of the current point in the map (start position of the robot). Its found with the nearest F of neighbors. The new current node is this one that have just found. The new node has also a pointer (reference) to its parent. This is the new node marked in the tree as “a”. So now found a new good point again. It is “c”. The “c” also has reference to “a”. Keep in mind that each node is the state of the matrix. Another new one and Its “e”. Unfortunately, it could push the diamond no more, because of the wall, because it was in the corner, many reasons could be. But it is not a problem. It just move up to the parent node and try again to find something interesting. But it is important to not check again the “e” node, because it is on the closed list. Situation is repeated constantly. Heuristic function will return grater F if it moves the diamond to the hole in the next step. Of course this is not as important to move only one diamond as to find a completed solution of the task. That is why is multiplied by 0.1. After creating each node, I just check if it is the same as destination goal. If it is, it breaks while loop statement and if not, try to search again. If there were many diamonds and a small map the tree will be more deeply than widely. If it was a big map, but the amount of diamonds will be small the tree will not be very deeply. Using algorithm like this, creates from 200 000 to 500 000 nodes. Some changes done in the balance with heuristic function so how many nodes have is really connected with the heuristic function. So it is really important to find good criteria in this function and in this case good balance between c, d and m. This small introduction was the AI planner. The algorithm is the same as the A* that is described in first chapter. Only definition of node will be different. The node will not contain x and y position, just a matrix state. Now it is not used A* algorithm to define way, another to turn with can. All is implemented in this A*like algorithm.
Second program:
There are three primary classes in this implementation.
· Map -This class represents a map.
· Node -This class represents each tile on the map. It has two primary methods.
o CompareTo -This method allows me to decide which node is better. We could say current node is better if this method returns negative number, which means current node, has lower cost than the other node being compared.
o isMatch -This allows me to decide whether two node’s geometrical positions are same or not.
· SortedCostNodeList -This is a list that stores Node object list. We needed to get off the lowest cost node from the list, so this list is implemented as sorted list order by cost value of the node, and it didn't need to examine the costs of all elements in the list every time to pop the node which had the lowest cost because they were already sorted. Just pop one. This list has two primary methods.
o push -add node elements to the list at proper position order by node cost.
o Node’s CompareTo method is used internally to sort order by cost.
o pop -just returns the lowest cost node from the list and remove it from the list.
Description of each class of the A* code:
Lets start describing the code. Once this is found useful, its added comments about what should have to be changed to solve all the Sokoban Game. First on the list of is Diamond Class. This class has not been used very often. It was connected with the matrix solution mentioned above.
Now it keeps position of diamonds and holes. Because it is .NET I defined some setters and getters.
Class Map is very important, used it very often. This class is uses every time algorithm makes small changes with the movements. The program has two matrixes. One to show the user situation, every time needed to show something board matrix. And the second one rewrites all from solution matrix mapdata. It has been decided to use it because mapdata has only digits and it is easier to use A* with numbers. Say the least firstly it keeps only G (F=G+H), but G from the nearest neighbor Function InitMap just init the mapdata. Function GetMap returns upper and down dimension of the matrix. PrintSolution is necessary to display the solution.
Now the main important Node Class. Because it is A* a kind of tree is updated every next movement. Firstly there was a thought about a structure (less memory is needed to keep one node in the tree). Due to implementations problems its been decided to use object as the node. The tree consists of nodes. In the first phase of the project it has only the code which is listed before. It changed that after into AI planner. Some functions say really what they do. For example totalCost returns the cost of the movement (G+H) public variables represents that cost. Variable leave diamond is connected with the robot. It says whether in this movement it leaves the diamond. Better saying it is connected with the part of the program that creates string parser of with commands that robot will have. Variables parent and _goalNode are used every movement after. During every movement, checked if the current node is the same as _goalNode. If it is, it means the solution is been found. Parent is used to keep the parent node of the current node. Every node without first one (root) has parent node. Function InitNode just calculate G and H cost for every node. CompareTo returns cFactor that is use in heuristic.
IsMatch means if object is matched to the second one. The last but not the least function is GetSuccessors. It adds successor’s neighbors’ to each position. We could use this Node Class with the AI planner and change only the representation of the state. Now it is position of the robot but it should be matrix representation Class NodeComparer is used to compare nodes in the tree. It has only one function to compare objects, the minus action of them is returned. Class Program is connected with .NET platform. It will not be described here. It is automatically generated. The same situation is with the partial class Form1.
RoadSolver is the really important class. This is the A* algorithm to move form point to point. We could use this class also as an AI planer but the Node class its been changed a little. It should not be represented only as robot position but as all matrixes -this is the change. Also if needed to be used as an AI planner it has to be defined start and goal node different.
Firstly the map is loaded, after that it is a definition of start and node goal. When the first node is pushed, the normal a* is started. After while loop that follow from parent node to the start node. You see it is inversion so to avoid this problem it pushed each node on the stack.
After that it had to be pop it with the right order. If it cannot follow from the end node to the start, it did not find solution. 5 will be returned. Please keep in mind that some functions in classes are static. So it does not have to talk about object. This class is static. So it was only created to separate code into small pieces. There was a road solver object available. In the end you can see a parser. As mentioned above, the node has only action of the movement. Look at the example below:
RRLL: It means that move one step right, again right after that left and left again. But robot cannot do that. It must have a variable connected with orientation and the parser must change the string into.
RSRRSS: The result of this action is the same: turn right, go straight, rotate (just turn to times) and go two times straight.
The last class SortedCostNodeList. It chose the lowest F in the next step in a*. It had then a kind of open and close list of nodes. This is that class. It has push, pop, index of and remove at functions to do basic operation on the list.
That is mainly described in the code of a*. Actually AI planner is not finished because limited time of the project. That is why it has been decided to put working code of a*. The AI planner of the code is made with some changes:
1. The class node was changed. Kept in node matrix not only x and y
2. Start and goal node is different, had to create a start and a point matrix and during while application compared if the matrix in the node is like the destination one. So it was the same idea of comparing node like it is now
3. GetAccessors() was a little bit different because it added all matrix that node has inside, but mainly it was distinction with node but not with this function
4. Everything inside RoadSolver if statement should be deleted:
if (tempPrevious!= null && tempPrevious.action!=null)
5. String parser will be different because we push the node with matrix state
6. That is all, because it is normal a*, had to just define other state in mainly saying Node.
NXT source code:
#pragma config(Sensor, S1, lightSensorL, sensorLightActive)
#pragma config(Sensor, S2, lightSensorR, sensorLightActive)
#pragma config(Sensor, S3, lightSensorF, sensorLightActive)
long left, right, straight;
int doneL, doneR, goS, trackL;
bool BlackOrWhite=0; //0 if black line, 1 if white
// List of actions to do by the robot
//kind of move l=Left r=Right s=go straight b=go back f=leave the diamond
char command[]= {‘l’,’s’,’r’,’s’,’s’,’r’,’s’,’s’,’s’,’s’,’s’,’s’,’b’,’b’,’f’ };
//List of prototypes
int NbOfMove=0;
int trackLine();
int turnLeft(int degree);
int turnRight(int degree);
int goStraight(int distance);
int stopRobot();
int GoToNext();
int GoBack();
// Main task which read the string of characters and decided what actions the robot has to do
// — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — —
task main()
{
doneL=0; doneR=0; goS=0; trackL=0;
left=0;right=0;straight=0;
//while its not at the arrival
while (command[NbOfMove]!=’f’)
{
if (command[NbOfMove]==”s”)
{
GoToNext();
}
if (command[NbOfMove]==”l”)
{
turnLeft(90);
}
if (command[NbOfMove]==”r”)
{
turnRight(90);
}
if (command[NbOfMove]==”b”)
{
GoBack();
}
NbOfMove++;
}
}
// — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — -
//Routine to make the robot follow the line.
//When the middle sensor doesn’t see the black line and one
//of the other see it, the robot turn on the right side in order to go back to the right position.
// — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — -
int trackLine ()
{
if (BlackOrWhite==1) // If line is white
{
if(SensorValue(lightSensorR) < 31 && SensorValue(lightSensorL)< 31 && SensorValue(lightSensorF)> 31)
{
motor[motorA] = 50;
motor[motorB] = 50;
}
if (SensorValue(lightSensorF)< 31)
{
if(SensorValue(lightSensorR)>31)
{
while(SensorValue(lightSensorF)< 31)
{
motor[motorB] = -40;
motor[motorA] = 40;
}
}
if(SensorValue(lightSensorL)>31)
{
while(SensorValue(lightSensorF)< 31)
{
motor[motorB] = 40;
motor[motorA] = -40;
}
}
}
}
else // If line is black
{
if(SensorValue(lightSensorR) > 31 && SensorValue(lightSensorL)> 31 && SensorValue(lightSensorF)< 31)
{
motor[motorA] = 50;
motor[motorB] = 50;
}
if (SensorValue(lightSensorF)> 31)
{
if(SensorValue(lightSensorR)❤1)
{
while(SensorValue(lightSensorF)> 31)
{
motor[motorB] = -30;
motor[motorA] = 30;
}
}
if(SensorValue(lightSensorL)❤1)
{
while(SensorValue(lightSensorF)> 31)
{
motor[motorB] = 30;
motor[motorA] = -30;
}
}
}
}
return 1;
}
// — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — -
//Routine to make the robot go to next intersection. The robot track the line during all the trip
// — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — -
int GoToNext()
{
if (BlackOrWhite==1)
{
while(!(SensorValue(lightSensorR) > 31 && SensorValue(lightSensorL)> 31 && SensorValue(lightSensorF)> 31))
{
trackLine();
}
}
else{
while(!(SensorValue(lightSensorR) < 31 && SensorValue(lightSensorL)< 31 && SensorValue(lightSensorF)< 31))
{
trackLine();
}
}
if (command[NbOfMove+1]!=”f”) // To leave the diamond in the middle of the intersection not after
{
goStraight(11);
}
return 1;
}
// — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — -
//Routine to turn the robot on the left. The value of the rotation is define by the parameter “degree”
// — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — -
int turnLeft(int degree)
{
long resultInDegree;
left=0;
resultInDegree= (long) 300*degree;
do
{
motor[motorA] = -40;
motor[motorB] = 40;
left++;
}while(left<resultInDegree);
trackLine();
return 1;
}
// — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — -
//Routine to turn the robot on the right. The value of the rotation is define by the parameter “degree”
// — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — -
int turnRight(int degree)
{
long resultInDegree;
right=0;
resultInDegree= (long) 300*degree;
do
{
motor[motorA] = 40;
motor[motorB] = -40;
right++;
}while(right<resultInDegree);
trackLine();
return 1;
}
// — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — -
//Routine to make the robot go straight. The value in centimeters is define by the parameter “degree”
// — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — -
int goStraight(int distance)
{
straight=0;
long resultInBricks;
resultInBricks= (long) 1400*distance;
do
{
motor[motorA] = 40;
motor[motorB] = 40;
straight++;
}while(straight<resultInBricks);
trackLine();
return 1;
}
// — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — -
//Routine to stop the robot.
// — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — -
int stopRobot()
{
motor[motorA] = 0;
motor[motorB] = 0;
return 1;
}
// — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — -
//Routine to make the robot go back to last intersection. The robot track the line during all the trip
// — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — — -
int GoBack()
{
if (BlackOrWhite==1)
{
while(!(SensorValue(lightSensorR) > 31 && SensorValue(lightSensorL)> 31 && SensorValue(lightSensorF)> 31))
{
if(SensorValue(lightSensorR) < 31 && SensorValue(lightSensorL)< 31 && SensorValue(lightSensorF)> 31)
{
motor[motorA] = -50;
motor[motorB] = -50;
}
if (SensorValue(lightSensorF)< 31)
{
if(SensorValue(lightSensorR)>31)
{
while(SensorValue(lightSensorF)< 31)
{
motor[motorB] = 40;
motor[motorA] = -40;
}
}
if(SensorValue(lightSensorL)>31)
{
while(SensorValue(lightSensorF)< 31)
{
motor[motorB] = -40;
motor[motorA] = 40;
}
}
}
}
}
else
{
while(!(SensorValue(lightSensorR) < 31 && SensorValue(lightSensorL)< 31 && SensorValue(lightSensorF)< 31))
{
if(SensorValue(lightSensorR) > 31 && SensorValue(lightSensorL)> 31 && SensorValue(lightSensorF)< 31)
{
motor[motorA] = -50;
motor[motorB] = -50;
}
if (SensorValue(lightSensorF)> 31)
{
if(SensorValue(lightSensorR)❤1)
{
while(SensorValue(lightSensorF)> 31)
{
motor[motorB] = 40;
motor[motorA] = -40;
}
}
if(SensorValue(lightSensorL)❤1)
{
while(SensorValue(lightSensorF)> 31)
{
motor[motorB] = -40;
motor[motorA] = 40;
}
}
}
}
}
straight=0;
long resultInBricks;
resultInBricks= (long) 1400*11;
do
{
motor[motorA] = -40;
motor[motorB] = -40;
straight++;
}while(straight<resultInBricks);
trackLine();
return 1;
}
Classes Code:
using System;
using System.Collections;
namespace Sokoban_Game
{
//this a Map class. This class has some static fields
//static fields mean that only one occurence of the object
//possibles.
public class Map
{
//I have to arrays. One is array of char [,]
//I used that array to show board on the Windows Form
//the second one is Mapdata, an array of int[,]
//Mapdata is used by algorithm a*
public static int[,] mapdata;
//I rewrite some data from array of chars (board)
//into array of integers
//Just keep in mind that we are interesting only about
//position of the wall, later I must think about
//other parts of the map, like diamonds etc…
public static void InitMap(char [,] board)
{
mapdata = new int[25,25];
for (int i=0;i<25;i++)
{
for (int j=0;j<25;j++)
{
//if I found a wall
if (board[j, i] == ‘X’) Map.mapdata[i, j] = -1;
if (board[j, i] == ‘J’) Map.mapdata[i, j] = 1;
}
}
}
//returns the map
public static int GetMap(int x, int y)
{
int yMax = mapdata.GetUpperBound(0);
int xMax = mapdata.GetUpperBound(1);
if (x < 0 || x > xMax)
return -1;
else if (y < 0 || y > yMax)
return -1;
else
return mapdata[y, x];
}
//
public static void PrintSolution(ArrayList solutionPathList)
{
int yMax = mapdata.GetUpperBound(0);
int xMax = mapdata.GetUpperBound(1);
for (int j = 0; j <= yMax; j++)
{
for (int i = 0; i <= xMax; i++)
{
bool solutionNode = false;
foreach (Node n in solutionPathList)
{
Node tmp = new Node(null, null, 1, i, j);
if (n.isMatch(tmp))
{
solutionNode = true;
break;
}
}
if (solutionNode)
mapdata[i, j] = 3;
}
}
}
}
}
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
namespace Sokoban_Game
{
class Diamonds
{
public int x {get;set;}
public int y {get;set;}
public string side { get; set; }
}
}
using System;
using System.Collections;
namespace Sokoban_Game
{
public class Node : IComparable
{
public Node() { }
//F=G+H this is the cost of the movement
public int totalCost
{
get
{
return g + h;
}
set
{
totalCost = value;
}
}
//cost moveing to and from the point
public int g;
public int h;
public bool leaveDiamond { get; set; }
public string action;
//place on the board
public int x;
public int y;
//matrix state representation should be here
//the node that its travelling to it
private Node _goalNode;
//in A* star solution parentNode
public Node parentNode;
//usually 1
private int gCost;
//constructor
public Node(Node parentNode, Node goalNode, int gCost, int x, int y)
{
this.parentNode = parentNode;
this._goalNode = goalNode;
this.gCost = gCost;
this.x = x;
this.y = y;
InitNode();
}
//initialization function
private void InitNode()
{
//calculate g costs
this.g = (parentNode != null) ? this.parentNode.g + gCost : gCost;
//manhatan cost
this.h = (_goalNode != null) ? (int)Manhatan_cost() : 0;
}
private double Manhatan_cost()
{
double xd = Math.Abs(this.x — this._goalNode.x);
double yd = Math.Abs(this.y — this._goalNode.y);
return (xd+yd);
}
public int CompareTo(object obj)
{
Node n = (Node)obj;
int cFactor = this.totalCost — n.totalCost;
return cFactor;
}
public bool isMatch(Node n)
{
if (n != null)
return (x == n.x && y == n.y);
else
return false;
}
public ArrayList GetSuccessors()
{
ArrayList successors = new ArrayList();
for (int xd = -1; xd <= 1; xd++)
{
for (int yd = -1; yd <= 1; yd++)
{
if (Map.GetMap(x + xd, y + yd) != -1)
{
if((xd==0 && yd==-1) ||
(xd==-1 && yd==0) ||
(xd== 1 && yd==0) ||
(xd== 0 && yd==1))
{
Node n = new Node(this, this._goalNode, Map.GetMap(x + xd, y + yd), x + xd, y +
yd);
if (xd == 0 && yd == -1) n.action = “UP”;
else if (xd == -1 && yd == 0) n.action = “LEFT”;
else if (xd == 1 && yd == 0) n.action = “RIGHT”;
else if (xd == 0 && yd == 1) n.action = “DOWN”;
if (!n.isMatch(this.parentNode) && !n.isMatch(this))
{
successors.Add(n);
}
}
}
}
}
return successors;
}
}
}
using System;
using System.Collections;
namespace Sokoban_Game
{
public class NodeComparer : IComparer
{
public NodeComparer()
{
}
public int Compare(object x, object y)
{
return ((Node)x).totalCost — ((Node)y).totalCost;
}
}
}
using System;
using System.Collections.Generic;
using System.Linq;
using System.Windows.Forms;
//this is main part of the programme
//it is like a main function in C
//I dont use this file because it is automatically generated
namespace Sokoban_Game
{
static class Program
{
/// <summary>
/// The main entry point for the application.
/// </summary>
[STAThread]
static void Main()
{
Application.EnableVisualStyles();
Application.SetCompatibleTextRenderingDefault(false);
Application.Run(new Form1());
}
}
}
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Windows.Forms;
using System.IO;
using System.Collections;
namespace Sokoban_Game
{
class RoadSolver
{
public static int RoadBetweenPoints(int x, int y, int X, int Y, int robotOrientation, TextWriter
tw, bool diamondHas)
{
Map.InitMap(Form1.board);
ArrayList SolutionPathList = new ArrayList();
//Create a node containing the goal state node_goal
Node node_goal = new Node(null, null, 1, X, Y);
//Create a node containing the start state node_start
Node node_start = new Node(null, node_goal, 1, x, y);
//Create OPEN and CLOSED list
SortedCostNodeList OPEN = new SortedCostNodeList();
SortedCostNodeList CLOSED = new SortedCostNodeList();
//Put node_start on the OPEN list
OPEN.push(node_start);
//while the OPEN list is not empty
while (OPEN.Count > 0)
{
//Get the node off the open list
//with the lowest f and call it node_current
Node node_current = OPEN.pop();
//if node_current is the same state as node_goal I have found the solution;
//break from the while loop;
if (node_current.isMatch(node_goal))
{
node_goal.parentNode = node_current;//.parentNode; //!!!!!!!!!!!!!!!!!!=============
break;
}
//Generate each state node_successor that can come after node_current
ArrayList successors = node_current.GetSuccessors();
//for each node_successor or node_current
foreach (Node node_successor in successors)
{
//Set the cost of node_successor to be the cost of node_current plus
//the cost to get to node_successor from node_current
// → already set while we are getting successors
//find node_successor on the OPEN list
int oFound = OPEN.IndexOf(node_successor);
//if node_successor is on the OPEN list but the existing one is as good
//or better then discard this successor and continue
if (oFound > 0)
{
Node existing_node = OPEN.NodeAt(oFound);
if (existing_node.CompareTo(node_current) <= 0)
continue;
}
//find node_successor on the CLOSED list
int cFound = CLOSED.IndexOf(node_successor);
//if node_successor is on the CLOSED list but the existing one is as good
//or better then discard this successor and continue;
if (cFound > 0)
{
Node existing_node = CLOSED.NodeAt(cFound);
if (existing_node.CompareTo(node_current) <= 0)
continue;
}
//Remove occurences of node_successor from OPEN and CLOSED
if (oFound != -1)
OPEN.RemoveAt(oFound);
if (cFound != -1)
CLOSED.RemoveAt(cFound);
//Set the parent of node_successor to node_current;
// → already set while we getting successors
//Set h to be the estimated distance to node_goal (Using heuristic function)
// → already set while we are getting successors
//Add node_successor to the OPEN list
OPEN.push(node_successor);
}
//Add node_current to the CLOSED list
CLOSED.push(node_current);
}
//follow the parentNode from goal to the start node
//to find a solution
Node p = node_goal;//.parentNode;//====================================================
p.leaveDiamond = true;
Stack answer = new Stack();
int foundSolution = 0;
while (p != null)
{
SolutionPathList.Insert(0, p);
answer.Push(p);
p = p.parentNode;
foundSolution ++;
}
if (foundSolution == 1)
{
Map.mapdata[node_goal.x, node_goal.y] = 5;
//!!!!!!!!!!!!!!!=========!!!!!!!!!!!!!!!!!!!!!
return 5;
}
Node tempPrevious;//=node_start; ;//= new Node();
//Node tempPPrevious = node_start;
//TextWriter sw = new StreamWriter(“x.txt”);
while (answer.Count != 0)
{
//tempPPrevious = tempPrevious;
tempPrevious = p;
p = (Node)answer.Pop();
if (tempPrevious != null && tempPrevious.action!=null)
{
if (tempPrevious.action != p.action && diamondHas == true)
{
//up
if ((p.action == “RIGHT” || p.action == “LEFT”) && robotOrientation == 1)
{
tw.Write(“F”);
//robotOrientation = 3;
diamondHas = false;
Form1.board[tempPrevious.x, tempPrevious.y] = ‘X’;
if (p.action == “RIGHT”)
{
if (RoadBetweenPoints(tempPrevious.x, tempPrevious.y + 1, tempPrevious.x -
1, tempPrevious.y, robotOrientation, tw, diamondHas) == 5)
return 5;
}
else if (p.action == “LEFT”)
{
if (RoadBetweenPoints(tempPrevious.x, tempPrevious.y + 1, tempPrevious.x +
1, tempPrevious.y, robotOrientation, tw, diamondHas) == 5)
return 5;
}
if (p.action==”RIGHT”) robotOrientation = 2;
else if (p.action == “LEFT”) robotOrientation = 4;
diamondHas = true;
tw.Write(“S”);
}
//right
else if ((p.action == “UP” || p.action == “DOWN”) && robotOrientation == 2)
{
tw.Write(“F”);
//robotOrientation = 4;
diamondHas = false;
Form1.board[tempPrevious.x, tempPrevious.y] = ‘X’;
if (p.action == “UP”)
{
if (RoadBetweenPoints(tempPrevious.x-1, tempPrevious.y, tempPrevious.x,
tempPrevious.y+1, robotOrientation, tw, diamondHas) == 5)
return 5;
}
else if (p.action == “DOWN”)
{
if (RoadBetweenPoints(tempPrevious.x-1, tempPrevious.y, tempPrevious.x,
tempPrevious.y-1, robotOrientation, tw, diamondHas) == 5)
return 5;
}
if (p.action == “UP”) robotOrientation = 1;
else if (p.action == “DOWN”) robotOrientation = 3;
diamondHas = true;
tw.Write(“S”);
}
else if ((p.action == “LEFT” || p.action == “RIGHT”) && robotOrientation == 3)
{
tw.Write(“F”);
//robotOrientation = 1;
diamondHas = false;
Form1.board[tempPrevious.x, tempPrevious.y] = ‘X’;
if (p.action == “RIGHT”)
{
if (RoadBetweenPoints(tempPrevious.x, tempPrevious.y-1, tempPrevious.x-1,
tempPrevious.y, robotOrientation, tw, diamondHas) == 5)
return 5;
}
else if (p.action == “LEFT”)
{
if (RoadBetweenPoints(tempPrevious.x, tempPrevious.y-1, tempPrevious.x+1,
tempPrevious.y, robotOrientation, tw, diamondHas) == 5)
return 5;
}
if (p.action == “LEFT”) robotOrientation = 4;
else if (p.action == “RIGHT”) robotOrientation = 2;
diamondHas = true;
tw.Write(“S”);
}
else if ((p.action == “UP” || p.action == “DOWN”) && robotOrientation == 4)
{
tw.Write(“F”);
//robotOrientation = 2;
diamondHas = false;
Form1.board[tempPrevious.x, tempPrevious.y] = ‘X’;
if (p.action == “UP”)
{
if (RoadBetweenPoints(tempPrevious.x+1, tempPrevious.y, tempPrevious.x,
tempPrevious.y+1, robotOrientation, tw, diamondHas) == 5)
return 5;
}
else if (p.action == “DOWN”)
{
if (RoadBetweenPoints(tempPrevious.x+1, tempPrevious.y, tempPrevious.x,
tempPrevious.y-1, robotOrientation, tw, diamondHas) == 5)
return 5;
}
if (p.action == “UP”) robotOrientation = 1;
else if (p.action == “DOWN”) robotOrientation = 3;
diamondHas = true;
tw.Write(“S”);
}
}
if (p.leaveDiamond != null && diamondHas == true)
{
if (p.leaveDiamond)
{
tw.Write(“F”);
//diamondHas = true;
}
}
//up
if (p.action == “UP” && robotOrientation == 1)
{
tw.Write(“S”);
robotOrientation = 1;
}
else if (p.action == “LEFT” && robotOrientation == 1)
{
tw.Write(“L”);
tw.Write(“S”);
robotOrientation = 4;
}
else if (p.action == “RIGHT” && robotOrientation == 1)
{
tw.Write(“R”);
tw.Write(“S”);
robotOrientation = 2;
}
else if (p.action == “DOWN” && robotOrientation == 1)
{
tw.Write(“R”);
tw.Write(“R”);
tw.Write(“S”);
robotOrientation = 3;
}
//right
else if (p.action == “UP” && robotOrientation == 2)
{
tw.Write(“L”);
tw.Write(“S”);
robotOrientation = 1;
}
else if (p.action == “LEFT” && robotOrientation == 2)
{
tw.Write(“R”);
tw.Write(“R”);
tw.Write(“S”);
robotOrientation = 4;
}
else if (p.action == “RIGHT” && robotOrientation == 2)
{
tw.Write(“S”);
robotOrientation = 2;
}
else if (p.action == “DOWN” && robotOrientation == 2)
{
tw.Write(“R”);
tw.Write(“S”);
robotOrientation = 3;
}
//down
else if (p.action == “UP” && robotOrientation == 3)
{
tw.Write(“R”);
tw.Write(“R”);
tw.Write(“S”);
robotOrientation = 1;
}
else if (p.action == “LEFT” && robotOrientation == 3)
{
tw.Write(“R”);
tw.Write(“S”);
robotOrientation = 4;
}
else if (p.action == “RIGHT” && robotOrientation == 3)
{
tw.Write(“L”);
tw.Write(“S”);
robotOrientation = 2;
}
else if (p.action == “DOWN” && robotOrientation == 3)
{
tw.Write(“S”);
robotOrientation = 3;
}
//left
else if (p.action == “UP” && robotOrientation == 4)
{
tw.Write(“R”);
tw.Write(“S”);
robotOrientation = 1;
}
else if (p.action == “LEFT” && robotOrientation == 4)
{
tw.Write(“S”);
robotOrientation = 4;
}
else if (p.action == “RIGHT” && robotOrientation == 4)
{
tw.Write(“R”);
tw.Write(“R”);
tw.Write(“S”);
robotOrientation = 2;
}
else if (p.action == “DOWN” && robotOrientation == 4)
{
tw.Write(“L”);
tw.Write(“S”);
robotOrientation = 3;
}
//tempPrevious = p;
//if (p.action == “DOWN”) sw.Write(“D”);
//else if (p.action == “UP”) sw.Write(“U”);
//lse if (p.action == “RIGHT”) sw.Write(“R”);
//else if (p.action == “LEFT”) sw.Write(“L”);
}
//sw.Close();
//display the solution
Map.PrintSolution(SolutionPathList);
for (int i = 0; i < Form1.height; i++)
for (int j = 0; j < Form1.width; j++)
if (Map.mapdata[j, i] == 3) Form1.board[j, i] = ‘w’;
return robotOrientation;
}
}
}
using System;
using System.Collections;
namespace Sokoban_Game
{
public class SortedCostNodeList
{
ArrayList _list;
NodeComparer _nodeComparer;
public int Count
{
get
{
return _list.Count;
}
}
public SortedCostNodeList()
{
_list = new ArrayList();
_nodeComparer = new NodeComparer();
}
public Node NodeAt(int i)
{
return (Node)_list[i];
}
public void RemoveAt(int i)
{
_list.RemoveAt(i);
}
public int IndexOf(Node n)
{
for (int i = 0; i < _list.Count; i++)
{
Node nodeInTheList = (Node)_list[i];
if (nodeInTheList.isMatch(n))
return i;
}
return -1;
}
public int push(Node n)
{
int k = _list.BinarySearch(n, _nodeComparer);
if (k == -1) // no element
_list.Insert(0, n);
else if (k < 0) // find location by complement
{
k = ~k;
_list.Insert(k, n);
}
else if (k >= 0)
_list.Insert(k, n);
return k;
}
public Node pop()
{
Node r = (Node)_list[0];
_list.RemoveAt(0);
return r;
}
}
}
