diff --git a/README.md b/README.md index 4b18076..9396e92 100644 --- a/README.md +++ b/README.md @@ -30,6 +30,15 @@ This is the repository of the [LPI](https://www.learningplanetinstitute.org/en), * Team formation + Ideation report * Project week - Project mainly focusing on solving the challenge while addressing odometry or kinematics problems. Implementation of your own algorithm to optimise the results. +## General Algorithm + +![WhatsApp Image 2023-01-05 at 02 10 01](https://user-images.githubusercontent.com/113441374/211695876-3de23d4a-08ad-44cb-9595-b7b99a7c9578.jpg) + +## A* Algorithm + +![image](https://user-images.githubusercontent.com/113441374/211696021-2a777b58-28ab-4869-af80-ab5d695bcebe.png) + + # Mini project guide Fork this repository and add your team members as collaborators to your fork. Make sure it is a public repository. diff --git a/lib/Flowchart_Astar algorithm.pdf b/lib/Flowchart_Astar algorithm.pdf new file mode 100644 index 0000000..e04ab5a Binary files /dev/null and b/lib/Flowchart_Astar algorithm.pdf differ diff --git a/lib/algorithm.md b/lib/algorithm.md new file mode 100644 index 0000000..f97ce10 --- /dev/null +++ b/lib/algorithm.md @@ -0,0 +1,73 @@ +# Algorithm +## Path finding and A* + +#### A* is a graph traversal and path search algorithm. Starting from a specific starting node of a graph, it aims to find apath to the given goal node having the smallest cost + +We are going to take this grid as an example: + +![image](https://user-images.githubusercontent.com/113441374/210691268-29917a55-1359-42c9-96c4-0a360b14ac95.png) + + +The grid has a start point, an end point and obstacles + +- First, it checks for neighbours. Because our robot can move up, +down, right or left, it checks for the neighbour that has +smallest cost. + +- The smallest cost, f is the sum of g and h, where: + - g = the movement cost to move from the starting point to a given +square on the grid, following the path generated to get there + - h = the estimated movement cost to move from that given square on the grid to +the final destination + +- At each iteration, the f value is calculated + +- It keeps track of the paths originating from the start node + +- The algorithm is extending the paths one grid cell at a time until its termination criterion is satisfied, which is meeting the end node + +## The steps involved in implementation of A* + +- Specifying the start and end node + +- Creating an open list to store all the nodes that can be taken into account when calculating the path + +- Creating a closed list that stores all the nodes that have been visited + +- We start with the start node and check its children (up, down, left and right) + +- We check if the children are in the maze boundaries, if the identify as an obstacle, or if they were already visited. If they meet any of these requirements, then we skip to the next child and do not calculate the f value, nor assign it to the open list. Otherwise, we calculate the g and h cost that form the final f. + +- We then check for the child with the lowest f and make this child the current node. We also append it to the closed list to form the path later + +- The iteration continues until the current node will have the same x and y positions as the end node. + +- The path will be returned taken into account the parents of the nodes. The path is calculated backwards, starting with the end point and tracing back the parents accordingly. + +## Flowchart of A* algorithm + + ![image](https://user-images.githubusercontent.com/113441374/210694023-ff1081c1-a971-48b0-8659-01b11a468859.png) + + +## Movment + +## Test the front + +![WhatsApp Image 2023-01-05 at 09 25 47](https://user-images.githubusercontent.com/113441374/210738497-08524a0e-cebb-4d09-93bc-6667d08010e6.jpg) + +## frontOpen + +![WhatsApp Image 2023-01-05 at 09 25 48](https://user-images.githubusercontent.com/113441374/210738582-f55d6433-0d8e-457a-8c01-411f206d52bc.jpg) + +## Go Forward + +![WhatsApp Image 2023-01-05 at 09 25 48](https://user-images.githubusercontent.com/113441374/210738674-e10dde17-e0c5-458c-9e5b-c02088b8faa9.jpg) + +## Turn left + +![WhatsApp Image 2023-01-05 at 09 25 47](https://user-images.githubusercontent.com/113441374/210738742-470fb530-3c18-4797-bcde-74f7c72a3066.jpg) + + +### General Algorithm of movment + +![WhatsApp Image 2023-01-05 at 02 10 02](https://user-images.githubusercontent.com/113441374/210738268-da515aec-b926-45a1-8071-9e2a88f34c6f.jpg) diff --git a/lib/ideation.md b/lib/ideation.md new file mode 100644 index 0000000..aa76ea7 --- /dev/null +++ b/lib/ideation.md @@ -0,0 +1,35 @@ +Ideation Report Template + +Project Name: Robot MHA + +Team: CRI project Link https://github.com/bmarid/robotics_course_2022 + +Mariia Bai, Andreea Stroia, Hala Albahloul + +1. Introduction : what do you want to do + Path planning robot. (8*8, 1 cell - 51 sm) + +2. Algorithm / Bibliography +A* algorithm + +3. How you will solve this problem +Code the map, Recognise obstacles, recalculate path, using A* algorithm. + +4. Expected List of Features +Code a Map +Recognise obstacles +Change/recalculate best path + +5. List of equipment needed (if we need to buy) +Arduino uno +H-bridge +Ultrasonic sensor +DC-motors +Jumpers +Battery +Robot pieces +Wheels + +6. References +https://www.youtube.com/watch?v=6TsL96NAZCo + diff --git a/lib/images/forward.jpeg b/lib/images/forward.jpeg new file mode 100644 index 0000000..fde4fbd Binary files /dev/null and b/lib/images/forward.jpeg differ diff --git a/lib/images/front_open.jpeg b/lib/images/front_open.jpeg new file mode 100644 index 0000000..32a327a Binary files /dev/null and b/lib/images/front_open.jpeg differ diff --git a/lib/images/general_movment_algorithm.jpeg b/lib/images/general_movment_algorithm.jpeg new file mode 100644 index 0000000..1671ba3 Binary files /dev/null and b/lib/images/general_movment_algorithm.jpeg differ diff --git a/lib/images/left.jpeg b/lib/images/left.jpeg new file mode 100644 index 0000000..946ec4d Binary files /dev/null and b/lib/images/left.jpeg differ diff --git a/lib/images/test_front.jpeg b/lib/images/test_front.jpeg new file mode 100644 index 0000000..232e140 Binary files /dev/null and b/lib/images/test_front.jpeg differ diff --git a/lib/resources.md b/lib/resources.md new file mode 100644 index 0000000..a6a1a72 --- /dev/null +++ b/lib/resources.md @@ -0,0 +1,15 @@ +# Resources + +### Using L298N Dual H-Bridge to control of DC motors: +https://dronebotworkshop.com/dc-motors-l298n-h-bridge/ + +### Using Ultrasonic sensor: +https://www.maxbotix.com/articles/how-ultrasonic-sensors-work.htm + +### A* Algorithm +https://www.youtube.com/watch?v=-L-WgKMFuhE&t=331s + +https://www.youtube.com/watch?v=6TsL96NAZCo + +https://www.geeksforgeeks.org/a-search-algorithm/ + diff --git a/lib/schematics/components.md b/lib/schematics/components.md new file mode 100644 index 0000000..248ea9d --- /dev/null +++ b/lib/schematics/components.md @@ -0,0 +1,55 @@ +# Circuit and Schematics +## 1. Circuit +![Copy of H-bridge motor driver](https://user-images.githubusercontent.com/113441374/210682973-18f31141-829d-4f7e-8ea8-1e3abdd9150d.png) + + +## 2. Components +- Arduino Uno. + +![image](https://user-images.githubusercontent.com/113441374/210687728-327f0895-2ed0-4d82-b56c-b8b7f33a53a9.png) + + +- L298N Dual H-bridge. + +- HC-SR04 Ultrasonic sensor. + +- 2 DC motors. + +![image](https://user-images.githubusercontent.com/113441374/210687852-0bfac715-8339-416a-8b22-6e41a906753c.png) + +- Battery 6v. + +![image](https://user-images.githubusercontent.com/113441374/210688150-caf0acda-b1c5-4c2b-9200-30cf57a4ed8c.png) + + +- BreadBoard. + +![image](https://user-images.githubusercontent.com/113441374/210687993-6f0da085-5ee1-4233-a764-671feb3f4a3c.png) + +- Wires. + + - ### L298N Dual H-bridge + + ![WhatsApp Image 2023-01-05 at 01 37 36](https://user-images.githubusercontent.com/113441374/210739374-1c4d257d-d582-4194-8653-66b8e8871443.jpg) + + An H-bridge is a simple circuit that lets you control a DC motor to go backward or forward. + +Changing the polarity of the power supply to DC motor is used to change the direction of rotation. +Apart from changing the rotation direction, the H-bridge can provide additional operation modes, "brake" and "free run until frictional stop". The H-bridge arrangement is generally used to reverse the polarity/direction of the motor, but can also be used to 'brake' the motor, where the motor comes to a sudden stop, as the motor's terminals are shorted + + - ### HC-SR04 Ultrasonic sensor + +![image](https://user-images.githubusercontent.com/113441374/210739285-c5b838a6-2a18-4c3e-858f-190f58706721.png) + + Ultrasonic sensors work by emitting sound waves at a frequency too high for humans to hear. then wait for the sound to be reflected back, calculating distance based on the time required. + calculated based on this formula: Distance = ½ T x C + + T = Time and C = the speed of sound + + ![WhatsApp Image 2023-01-05 at 01 28 59](https://user-images.githubusercontent.com/113441374/210690752-99c998a2-366d-4235-aff9-a494c9369873.jpg) + + ![WhatsApp Image 2023-01-05 at 01 29 23](https://user-images.githubusercontent.com/113441374/210690803-d6ab0ac6-a0ac-416a-95bb-29af6868a3ea.jpg) + + + + diff --git a/src/A* algorithm C b/src/A* algorithm C new file mode 100644 index 0000000..6de07ff --- /dev/null +++ b/src/A* algorithm C @@ -0,0 +1,183 @@ +#include +#include + +//Defining the start and end nodes +int start_x =1; +int start_y=0; +int end_y=5; +int end_x=9; + +//Creating a struct to store all the nodes that the robot can take + +typedef struct { + int x; //position of x + int y; //position of y + double f; + double g; + double h; +}Node; + +typedef struct{ + Node info; + struct List_node* next; +}List_node; + + +// Creating a simple list to store the nodes + +List_node* creation(List_node* new_node, List_node* startnode, Node node, List_node* endnode) { + + new_node->info.x = node.x; + new_node->info.y = node.y; + new_node->info.g = abs((new_node->info.x - startnode->info.x) + (new_node->info.y - startnode->info.y)); + new_node->info.h = abs((new_node->info.x - endnode->info.x) + (new_node->info.y - endnode->info.y)); + new_node->info.f = new_node->info.g + new_node->info.h; + + return new_node; +} + +//Creating a function to insert new nodes + +List_node* inseration(List_node* startnode, Node node, List_node* endnode) +{ + List_node* new_node = (List_node*)malloc(sizeof(List_node)); + new_node = creation(new_node, startnode, node, endnode); + + if (startnode) + { + List_node* aux = startnode; + while (aux->next) + aux = aux->next; + aux->next = new_node; + } + else + startnode = new_node; + + int min = 10000; + int count = 0; + + //Allocating memory for the new nodes the robot can take + //It can go left, right, down and up + List_node* left_node = (List_node*)malloc(sizeof(List_node)); + List_node* up_node = (List_node*)malloc(sizeof(List_node)); + List_node* right_node = (List_node*)malloc(sizeof(List_node)); + List_node* down_node = (List_node*)malloc(sizeof(List_node)); + + //The robot goes left + if(node.x > 0) { // + Node nodeLeft; + nodeLeft = node; + nodeLeft.x = node.x - 1; + left_node = creation(left_node, startnode, nodeLeft, endnode); + if(left_node->info.f < min) + { + min = left_node->info.f; + count = 4; + } + } + + //The robot goes uo + if(node.y > 0) { + Node nodeUp; + nodeUp = node; + nodeUp.y = node.y + 1; + up_node = creation(up_node, startnode, nodeUp, endnode); + if(up_node->info.f < min) + { + min = up_node->info.f; + count = 1; + } + } + + //The robot goes right + if(node.x < 7) { + Node nodeRight; + nodeRight = node; + nodeRight.x = node.x + 1; + right_node = creation(right_node, startnode, nodeRight, endnode); + if(right_node->info.f < min) + { + min = right_node->info.f; + count = 2; + } + } + + //The robot goes down + if(node.y < 7) { + Node nodeDown; + nodeDown = node; + nodeDown.y = node.y - 1; + down_node = creation(down_node, startnode, nodeDown, endnode); + if(down_node->info.f < min) + { + min = down_node->info.f; + count = 3; + } + } + + //The robot goes up + if(count == 1) { + new_node->next = up_node; + } else if (count == 2) { + new_node->next = right_node; + } else if (count == 3) { + new_node->next = down_node; + } else { + new_node->next = left_node; + } + + if(new_node->info.x != end_x && new_node->info.y != end_y) + return startnode; + else + inseration(startnode, node, endnode); +} + +void path(List_node* head) +{ + List_node* aux = head; + while (aux) + { + printf("\n(%d, %d) ", + aux->info.x, aux->info.y); + aux = aux->next; + } +} + +void main() +{ + //Initializing the end node + + List_node *Endnode; + Endnode->info.x = end_x; + Endnode->info.y = end_y; + Endnode->info.g = 0; + Endnode->info.h = 0; + Endnode->info.f = 0; + Endnode->next = NULL; + + int maze[8][8] = + {{0, 0, 1, 0, 0, 0, 0, 0}, + { 0, 0, 1, 0, 0, 0, 0, 0}, + { 0, 0, 1, 0, 0, 0, 0, 0}, + { 0, 0, 1, 0, 0, 0, 0, 0}, + { 0, 0, 1, 0, 0, 0, 0, 0}, + { 0, 0, 0, 0, 0, 0, 0, 0}, + { 0, 0, 1, 0, 0, 0, 0, 0}, + {0, 0, 1, 0, 0, 0, 0, 0}}; + + for(int i = 0; i < 8; i++) + for(int j = 0; j < 8; j++) { + + List_node *Startnode; + Node node; + node.x = start_x; + node.y = start_y; + node.g = 0; + node.h = 0; + node.f = 0; + Startnode->info = node; + Startnode->next = NULL; + + Startnode = inseration(Startnode, node, Endnode); + } +} diff --git a/src/A* algorithm Python b/src/A* algorithm Python new file mode 100644 index 0000000..51cc634 --- /dev/null +++ b/src/A* algorithm Python @@ -0,0 +1,127 @@ + +#A* algorithm +#Goal:finding the best path using A* algorithm + +#We store all the children to be evaluated +open_list=[] +#We store the nodes that will form the path +closed_list=[] + +start_x=0 +start_y=0 +end_y=7 +end_x=7 + + +maze=[] + +#Initializing random obstacle list +obstacle_list =[(0,3),(4,5),(5,5),(6,5),(7,5)] + +class Node(): + def __init__(self,x,y, parent = None): + self.parent = parent + self.x = x + self.y = y + self.f = 0 + self.g = 0 + self.h = 0 + +def astar(maze): + #Initialing the start and end nodes + StartNode = Node(start_x, start_y, None) + EndNode = Node(end_x,end_y, None) + + current = StartNode + open_list.append(current) + closed_list.append(current) + + path = [] + counter = 0 + + #Defifining the position that the child can take: up, down, left, right + child_list=[(0,1),(0,-1),(-1,0),(1,0)] + + while(len(open_list) != 0): + # counter = counter + 1 + # if (counter > 2): + # open_list.clear() + # break + + #Setting a high value for lowest f in order for it to start taking the f values of children + lowest_f = 65 + + if current in open_list: + open_list.remove(current) + + #Looping through the children + for i in child_list: + obstacle=0 + childNode = Node(current.x+i[0],current.y+i[1], current) + # childNode = Child(1,2) + #print(childNode.x, childNode.y) + + # Checking if it exists in the closed list + if childNode in closed_list: + continue + #Checking if it is an obstacle + for t in obstacle_list: + if childNode.x == t[0] and childNode.y == t[1]: + obstacle = 1 + if obstacle == 1: + continue + + # Checking if it is in the maze + if childNode.x > 7 or childNode.x < 0 or childNode.y > 7 or childNode.y < 0: + continue + + # Checking if it is in open list + if childNode in open_list: + continue + + #Calculating values for f, g, h + childNode.g = abs(childNode.x-StartNode.x)+abs(childNode.y-StartNode.y) + childNode.h = abs(childNode.x-EndNode.x)+abs(childNode.y-EndNode.y) + childNode.f = childNode.g + childNode.h + + #Appending in open list + open_list.append(childNode) + + # Checking which child in open list has the lowest f and assigning the next node to be evaluated for lowest f + for r in open_list: + if r.f < lowest_f: + current = r + lowest_f = r.f + #print(lowest_f) + elif r.f == lowest_f and r.h < current.h: + current = r + + closed_list.append(current) + + #Checking if it is end Node .List will become empty and the while will be broken + if current.x == EndNode.x and current.y == EndNode.y: + path = [] + path.append((current.x, current.y)) + while (current.parent != None): + current = current.parent + path.append((current.x, current.y)) + open_list.clear() + return (path[::-1]) + +def main(): + + maze = [[ 0, 0, 0, 0, 0, 0, 0, 0], + [ 0, 0, 0, 0, 0, 0, 0, 0], + [ 0, 0, 0, 0, 0, 0, 0, 0], + [ 0, 0, 0, 0, 0, 0, 0, 0], + [ 0, 0, 0, 0, 0, 0, 0, 0], + [ 0, 0, 0, 0, 0, 0, 0, 0], + [ 0, 0, 0, 0, 0, 0, 0, 0], + [0, 0, 0, 0, 0, 0, 0, 0]] + + + path = astar(maze) + print(path) + +if __name__ == '__main__': + main() diff --git a/src/Astar/Astar_C_final_version.c b/src/Astar/Astar_C_final_version.c new file mode 100644 index 0000000..d494e48 --- /dev/null +++ b/src/Astar/Astar_C_final_version.c @@ -0,0 +1,185 @@ +#include +#include + +//A* algorithm + + +//Creating a struct to store all proprieties the node has + +typedef struct { + int x; //position of x + int y; //position of y + double f; + double g; + double h; +}Node; + +//Creating a list of structs to store the open list +typedef struct{ + Node info; + struct List_node* next; +}List_node; + +//Creating a linked list to add the first element of the list (start node) + +List_node* Add(List_node *next_node, Node node, int start_x, int start_y, int end_x, int end_y) //this will return us a list +{ + List_node * new_node; + new_node = (List_node*)malloc(sizeof(List_node)); + new_node->info=node; + new_node->next=next_node;//the node's new value +} + +// Creating the possible children the nodes can take +//This function returns the node that has the lowest f value checking for all the possible children the node can take +Node thebest(Node node, int start_x, int start_y, int end_x, int end_y) +{ + + int pos= 1; //The robot can go just 1 position from the current cell + int min = 1000; //This value is used to compare between the f values + //int count =0; + + Node counter = node; + Node nodeLeft; + nodeLeft = node; + + //Calculating the position + //The robot goes left + int adding = node.x-pos; //calculates the next position + nodeLeft.x=adding; + nodeLeft.y=node.y; + nodeLeft.g=abs(nodeLeft.x-start_x)+abs(nodeLeft.y-start_y); + nodeLeft.h=abs(nodeLeft.x-end_x)+abs(nodeLeft.y-end_y); + nodeLeft.f=nodeLeft.g+nodeLeft.h; + if(nodeLeft.f < counter.f) + { + printf("left"); + min = nodeLeft.f; + counter=nodeLeft; + //count = 4; + } + //The robot goes up + Node nodeUp; + nodeUp = node; + nodeUp.x=node.x; + int adding2 = node.y+pos; //calculates the next position + nodeUp.y=adding2; + nodeUp.g=abs(nodeUp.x-start_x)+abs(nodeUp.y-start_y); + nodeUp.h=abs(nodeUp.x-end_x)+abs(nodeUp.y-end_y); + nodeUp.f=nodeUp.g+nodeUp.h; + + if(nodeUp.f < counter.f) + { + printf("up"); + min = nodeUp.f; + counter=nodeUp; + //count = 1; + } + //The robot goes right + Node nodeRight; + nodeRight = node; + nodeRight.y = node.y; + int adding3 = node.x+pos; //calculates the next position + nodeRight.x=adding3; + nodeRight.g=abs(nodeRight.x-start_x)+abs(nodeRight.y-start_y); + nodeRight.h=abs(nodeRight.x-end_x)+abs(nodeRight.y-end_y); + nodeRight.f=nodeRight.g+nodeRight.h; + + if(nodeRight.f < counter.f) + { + printf("right"); + min = nodeRight.f; + counter=nodeRight; + //count = 2; + } + //The robot goes down + Node nodeDown; + nodeDown = node; + nodeDown.x = node.x; + int adding4 = node.y-pos; //calculates the next position + nodeDown.y=adding4; + nodeDown.g=abs(nodeDown.x-start_x)+abs(nodeDown.y-start_y); + nodeDown.h=abs(nodeDown.x-end_x)+abs(nodeDown.y-end_y); + nodeDown.f=nodeDown.g+nodeDown.h; + + if(nodeDown.f < counter.f) + { + printf("down"); + min = nodeDown.f; + counter=nodeDown; + //count = 3; + } + return counter; //this functions returns the child with the lowest f + +} + +List_node *Addchildren(List_node* new_node, Node node, int start_x, int start_y, int end_x, int end_y ) +{ + Node simple; + List_node *thebest1=(List_node*)malloc(sizeof(List_node)); + simple=thebest(node, start_x, start_y, end_x, end_y); + thebest1->info=simple; + thebest1->next=new_node; + + if(thebest1->info.x != end_x && thebest1->info.y != end_y) + { + int path_found=1; + thebest1->next=NULL; + } + else + return thebest1->next=new_node; +} + +void main() +{ + //Defining the start and end nodes + int path_found =0; + int start_x =0; + int start_y=0; + int end_y=5; + int end_x=9; + //Initializing startnode + Node Startnode; + Startnode.x = start_x; + Startnode.y=start_y; + Startnode.f=0; + Startnode.g=0; + Startnode.h=0; + + List_node *node=NULL; + List_node *node2=NULL; //list initializing + + node = Add(node, Startnode, start_x, start_y, end_x, end_y); + node2 = Addchildren(node, Startnode, start_x, start_y, end_x, end_y); + + int maze[8][8] = + {{0, 0, 1, 0, 0, 0, 0, 0}, + { 0, 0, 1, 0, 0, 0, 0, 0}, + { 0, 0, 1, 0, 0, 0, 0, 0}, + { 0, 0, 1, 0, 0, 0, 0, 0}, + { 0, 0, 1, 0, 0, 0, 0, 0}, + { 0, 0, 0, 0, 0, 0, 0, 0}, + { 0, 0, 1, 0, 0, 0, 0, 0}, + {0, 0, 1, 0, 0, 0, 0, 0}}; + + for(int i = 0; i < 8; i++) + for(int j = 0; j < 8; j++) { + + List_node *iList = node2; + Node current; + if ((i == iList->info.x)&&(j==iList->info.y)){ + current.x=i; + current.y=i; + node2 = Addchildren(node, current, start_x, start_y, end_x, end_y); + } + + while(iList!=0) + { + printf("%d ", iList->info.x); + printf("%d ", iList->info.y); + iList = iList ->next; + } + + return 0; +} +} diff --git a/src/Astar/Astar_Python.py b/src/Astar/Astar_Python.py new file mode 100644 index 0000000..0204071 --- /dev/null +++ b/src/Astar/Astar_Python.py @@ -0,0 +1,129 @@ + +#A* algorithm + +#We store all the children to be evaluated +open_list=[] +#We store the nodes that form the path +closed_list=[] + +start_x=0 +start_y=0 +end_y=7 +end_x=7 + +# print(StartNode) +# print(open_list) +maze=[] + +obstacle_list =[(0,3),(4,5),(5,5),(6,5),(7,5)] + +class Node(): + def __init__(self,x,y, parent = None): + self.parent = parent + self.x = x + self.y = y + self.f = 0 + self.g = 0 + self.h = 0 + +def astar(maze): + #Initialing the start and end nodes + StartNode = Node(start_x, start_y, None) + EndNode = Node(end_x,end_y, None) + + current = StartNode + open_list.append(current) + closed_list.append(current) + + #A list to store the final path + path = [] + counter = 0 + + #Defifining the position that the child can take: up, down, left, right + child_list=[(0,1),(0,-1),(-1,0),(1,0)] + + while(len(open_list) != 0): + # counter = counter + 1 + # if (counter > 2): + # open_list.clear() + # break + + #Setting a high value for lowest f in order for it to start taking the f values of children + lowest_f = 65 + + if current in open_list: + open_list.remove(current) + + #Looping through the children + for i in child_list: + obstacle=0 + childNode = Node(current.x+i[0],current.y+i[1], current) + # childNode = Child(1,2) + #print(childNode.x, childNode.y) + + # Checking if it exists in the closed list + if childNode in closed_list: + continue + #Checking if it is an obstacle + for t in obstacle_list: + if childNode.x == t[0] and childNode.y == t[1]: + obstacle = 1 + if obstacle == 1: + continue + + # Checking if it is in the maze + if childNode.x > 7 or childNode.x < 0 or childNode.y > 7 or childNode.y < 0: + continue + + # Checking if it is in open list + if childNode in open_list: + continue + + #Calculating values for f, g, h + childNode.g = abs(childNode.x-StartNode.x)+abs(childNode.y-StartNode.y) + childNode.h = abs(childNode.x-EndNode.x)+abs(childNode.y-EndNode.y) + childNode.f = childNode.g + childNode.h + + #Appending in open list + open_list.append(childNode) + + # Checking which child in open list has the lowest f and assigning the next node to be evaluated for lowest f + + for r in open_list: + if r.f < lowest_f: + current = r + lowest_f = r.f + elif r.f == lowest_f and r.h < current.h: + current = r + + closed_list.append(current) + + + #Checking if it is end Node .List will become empty and the while will be broken + if current.x == EndNode.x and current.y == EndNode.y: + path = [] + path.append((current.x, current.y)) + while (current.parent != None): + current = current.parent + path.append((current.x, current.y)) + open_list.clear() + return (path[::-1]) + # break + +def main(): + + maze = [[ 0, 0, 1, 0, 0, 0, 0, 0], + [ 0, 0, 1, 0, 0, 0, 0, 0], + [ 0, 0, 1, 0, 0, 0, 0, 0], + [ 0, 0, 1, 0, 0, 0, 0, 0], + [ 0, 0, 1, 0, 0, 0, 0, 0], + [ 0, 0, 0, 0, 0, 0, 0, 0], + [ 0, 0, 1, 0, 0, 0, 0, 0], + [0, 0, 1, 0, 0, 0, 0, 0]] + + + path = astar(maze) + print(path) + +if __name__ == '__main__': + main() diff --git a/src/Robot_movment/Robot_movment.ino b/src/Robot_movment/Robot_movment.ino new file mode 100644 index 0000000..f18493a --- /dev/null +++ b/src/Robot_movment/Robot_movment.ino @@ -0,0 +1,394 @@ + +int enA = 9; +int in1 = 8; +int in2 = 7; + +// Motor B + +int enB = 3; +int in3 = 5; +int in4 = 4; + +//Ultrasonic +int trigPin = 11; // Trigger +int echoPin = 10; // Echo +long duration, cm; + + +bool goal = false; + +// 0 is up +// 1 is right +// 2 is down +// 3 is left +int direction = 2; + + +int i = 2; +int j = 0; + +int path[8][8] = +{ + { 1,0,0,1,1,1,1,1 } , + { 1,1,0,1,1,1,1,1 } , + { 1,1,0,1,1,1,1,1 } , + { 1,1,0,1,1,1,1,1 } , + { 1,1,0,0,1,1,1,1 } , + { 1,1,1,-1,1,1,1,1 } , + { 1,1,1,1,1,1,1,1 } , + { 1,1,1,1,1,1,1,1 } +}; + +void setup() + +{ + //Ulterasonic + //Serial Port begin + Serial.begin(9600); + //Define inputs and outputs pins for ultrasonic + pinMode(trigPin, OUTPUT); + pinMode(echoPin, INPUT); + + // Set all the motor control pins to outputs + + pinMode(enA, OUTPUT); + pinMode(enB, OUTPUT); + pinMode(in1, OUTPUT); + pinMode(in2, OUTPUT); + pinMode(in3, OUTPUT); + pinMode(in4, OUTPUT); + +} + +/** + turns on the motors to go forward. + + @param colorCode color code to convert. + @return the numerical value of the color code. +*/ + +void forward() { + + // Turn on motor B and A to go forward + digitalWrite(in1, HIGH); + digitalWrite(in2, LOW); + digitalWrite(in3, HIGH); + digitalWrite(in4, LOW); + + // Set the speed value out of possible range 0~255 + analogWrite(enB, 200); + analogWrite(enA, 182); + delay(1800); + + Serial.println("Forward"); + + //Change the indexs on the path matrix after the robot move + if (direction == 0) + j = j - 1; + if (direction == 1) + i = i + 1; + if (direction == 2) + j = j + 1; + if (direction == 3) + i = i - 1; +} + + + +void left() { + + // Turn on motor B and A to turn to the left + digitalWrite(in1, HIGH); + digitalWrite(in2, LOW); + digitalWrite(in3, LOW); + digitalWrite(in4, HIGH); + + delay(170); + Serial.println("Left"); + + // Change the direction of the robot after it turn left + if (direction == 0) + direction = 3; + else if (direction == 1) + direction = 0; + else if (direction == 2) + direction = 1; + else if (direction == 3) + direction = 2; + +} + + +void right() { + + // Turn on motor B and A to turn to the right + digitalWrite(in1, LOW); + digitalWrite(in2, HIGH); + digitalWrite(in3, HIGH); + digitalWrite(in4, LOW); + //analogWrite(enB, 200); + // analogWrite(enA, 200); + delay(200); + Serial.println("Right"); + + if (direction == 0) + direction = 1; + else if (direction == 1) + direction = 2; + else if (direction == 2) + direction = 3; + else if (direction == 3) + direction = 0; +} + +void stop_robot() { + //Stop the motors + digitalWrite(in1, LOW); + digitalWrite(in2, LOW); + digitalWrite(in3, LOW); + digitalWrite(in4, LOW); + delay(2000); +} + + +void turn180() { + + Serial.println("Around"); + digitalWrite(in1, HIGH); + digitalWrite(in2, LOW); + digitalWrite(in3, LOW); + digitalWrite(in4, HIGH); + analogWrite(enB, 200); + analogWrite(enA, 175); + delay(500); + if (direction == 0) + direction = 2; + else if (direction == 1) + direction = 3; + else if (direction == 2) + direction = 0; + else if (direction == 3) + direction = 1; + + +} +/** + Calculate the distance in the front of the robot. + + @param None. + @return a the distance in cm. +*/ +long calcDistance() {/////most change the comments--------------------------------- + + // The sensor is triggered by a HIGH pulse of 10 or more microseconds. + // Give a short LOW pulse beforehand to ensure a clean HIGH pulse: + digitalWrite(trigPin, LOW); + delayMicroseconds(5); + digitalWrite(trigPin, HIGH); + delayMicroseconds(10); + digitalWrite(trigPin, LOW); + + // Read the signal from the sensor: a HIGH pulse whose + // duration is the time (in microseconds) from the sending + // of the ping to the reception of its echo off of an object. + pinMode(echoPin, INPUT); + duration = pulseIn(echoPin, HIGH); + + // Convert the time into a distance + cm = (duration / 2) / 29.1; // Divide by 29.1 or multiply by 0.0343 + + Serial.print(cm); + Serial.print("cm"); + Serial.println(); + delay(300); + + return cm; +} + + +int test_front() { + if (direction == 0) { + return path[j - 1][i]; + } + if (direction == 1) { + return path[j][i + 1]; + } + if (direction == 2) { + return path[j + 1][i]; + } + if (direction == 3) { + return path[j][i - 1]; + } +} +int return_indexJ() { + if (direction == 0) { + return j - 1; + } + if (direction == 1 || direction == 3) { + return j; + } + if (direction == 2) { + return j + 1; + } + +} +int return_indexI() { + if (direction == 0 || direction == 2) { + return i; + } + if (direction == 1) { + return i + 1; + } + + if (direction == 3) { + return i - 1; + } +} + + + +/** + Gets the number on the Grid of the space to the Right of it. + + @param None. + @return the vlaue of the grid. +*/ +int test_right() { + if (direction == 0) { + return path[j][i + 1]; + } + if (direction == 1) { + return path[j + 1][i]; + } + if (direction == 2) { + return path[j][i - 1]; + } + if (direction == 3) { + return path[j - 1][i]; + } +} + + + +/** + Gets the number on the Grid of the Space to the Left of it. + + @param None. + @return the vlaue of the grid. +*/ +int test_left() { + if (direction == 0) { + return path[j][i - 1]; + } + if (direction == 1) { + return path[j - 1][i]; + } + if (direction == 2) { + return path[j][i + 1]; + } + if (direction == 3) { + return path[j + 1][i]; + } +} + + +/** + Gets the number on the Grid of the Space to the Left of it. + + @param None. + @return 1 if the front of the robot is closed due to the grid and the vaue of the grid if the front is open. +*/ +int frontOpen() { + int next = test_front(); + if (next == 0) { + return next; + } + else if (next == -1) { + goal = true; + return next; + } + + else { + return 1; + } +} + +/** + Gets the number on the Grid of the Space to the Left of it. + + @param None. + @return True if the right of the robot is Open (free) due to the grid and False if it is closed. +*/ +boolean rightOpen() { + int next = test_right(); + if (next == 0 || next == -1) { + return true; + } + else { + return false; + } +} + +/** + Gets the number on the Grid of the Space to the Left of it. + + @param None. + @return True if the left of the robot is Open (free) due to the grid and False if it is closed. +*/ +boolean leftOpen() { + int next = test_left(); + if (next == 0 || next == -1) { + return true; + } + else { + return false; + } +} + +void set_obstecal() { + int y= return_indexI(); + int x=return_indexJ(); + //recalc +} + + +void loop() { + + if (calcDistance() < 35) { + stop_robot(); + set_obstecal(); + if (rightOpen() == true) { + right(); + } + else if (leftOpen() == true) { + left(); + } + else { + turn180(); + + } + //recalc path---------------------------------------------- + } + else { + //delay(400); + if (frontOpen() == 0 || frontOpen() == -1) { + forward(); + stop_robot(); + //delay(1000); + if (goal==true) { + stop_robot(); + delay(100000); + } + } + else if (rightOpen() == true) { + right(); + } + else if (leftOpen() == true) { + left(); + } + else { + turn180(); + + } + } +} diff --git a/src/full_workable_version_1/full_workable_version_1.ino b/src/full_workable_version_1/full_workable_version_1.ino new file mode 100644 index 0000000..4a67a32 --- /dev/null +++ b/src/full_workable_version_1/full_workable_version_1.ino @@ -0,0 +1,877 @@ +//—--- Mari car +// Motor A +//int enA = 9; +//int in1 = 2; +//int in2 = 3; + +// Motor B +////int enB = 10; +//int in3 = 4; +//int in4 = 5; + +//Ultrasonic +//int trigPin = 12; // Trigger +//int echoPin = 11; // Echo +//long duration, cm; + +//—--- Hala car +int enA = 9; +int in1 = 8; +int in2 = 7; + +// Motor B + +int enB = 3; +int in3 = 5; +int in4 = 4; + +//Ultrasonic +int trigPin = 11; // Trigger +int echoPin = 10; // Echo +long duration, cm; +//---------------------HALA car-----------------------keep + +int goal = 0; + +// 0 is up +// 1 is right +// 2 is down +// 3 is left +int direction = 2; +// the point in the grid where the robot is +//TODO: take variable from start_x, start_y +int i = 0; +int j = 0; +// we will have 2 matrix +//int path[8][8] = +//{ +// { 1,0,0,1,1,1,1,1 } , +// { 1,1,0,1,1,1,1,1 } , +// { 1,1,0,1,1,1,1,1 } , +// { 1,1,0,1,1,1,1,1 } , +// { 1,1,0,0,1,1,1,1 } , +// { 1,1,1,-1,1,1,1,1 } , +// { 1,1,1,1,1,1,1,1 } , +// { 1,1,1,1,1,1,1,1 } +//}; + + +// path part start +#define OBST 99 +#define beautiful_output 1 //beautiful output +// +////Test 1 normal 8 8 +#define map_size_rows 8 +#define map_size_cols 8 +// +int ind[map_size_rows][map_size_cols] = { + {0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0} +}; + +int path[map_size_rows][map_size_cols] = { + {0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0} +}; + +//this is to see data during testing process +int test = 0; +int test_main = 0; +int test_main_path = 0; +int test_main_display = 0; +int test_arduino = 0; //for arduino functions to show +//int test_main_path_arduino = 1; + +int start = 1; //start point +int start_x = 0; //vertical +int start_y = 0; //horisontal + +int goal_target = -1; //TODO: FINALLY REWRITE FOR PATH +int goal_x = map_size_rows - 1; +int goal_y = map_size_cols - 1; + +//int goal_x = 7; int goal_y = 1; //test left goal +int path_max_value = 0; +int calc_matrix_wave = 1; // 1 - calculated? + +// CHANGE output for arduino +void pr(int value) { +// printf("%2d",value); + Serial.print(" "); + Serial.print(value); +} + +void pr_s(char value[]) { +// printf(value); + Serial.print(" "); + Serial.print(value); +} + +void ln(void){ +// printf("\n"); + Serial.println(); +} + +int update_matrix_with(int matrix[map_size_rows][map_size_cols], int x, int y, int new_value){ + //check if position exist + if(x >= 0 && x <= map_size_rows && y >= 0 && y <= map_size_cols) { + matrix[x][y] = new_value; + } + else { + pr_s("Can't update, position doesn't exist"); + pr(x); pr_s(","); pr(y); ln(); + } + return matrix[x][y]; +} + +void insert_obstcls(int ind[map_size_rows][map_size_cols], int obst){ + update_matrix_with(ind, 1, 1, OBST); + update_matrix_with(ind, 1, 2, OBST); +} + +void ind_first_line(void) { + pr_s(".."); + for(int i = 0; i < map_size_cols; i++) { + pr(i); + pr_s(" "); + } + ln(); +} + +void show_matrix(int matrix[map_size_rows][map_size_cols]){ + if(beautiful_output) ind_first_line(); + for(int i = 0; i < map_size_rows; i++){ + if(beautiful_output) pr(i); + for(int j = 0; j < map_size_cols; j++){ + pr(matrix[i][j]); + pr_s(" "); + } ln(); + } ln(); +} + +void show_matrix_revert(int matrix[map_size_rows][map_size_cols]){ + if(beautiful_output) { + pr_s(".. "); + for(int i = map_size_rows - 1; i >= 0; i--) + pr(i); + ln(); + } + for(int i = map_size_rows - 1; i >= 0; i--) { + if(beautiful_output) { + pr(i); + pr_s(":"); + } + for(int j = map_size_rows - 1; j >= 0; j--) { + pr(matrix[i][j]); + } + ln(); + } ln(); +} + +void replace_all(int matrix[map_size_rows][map_size_cols], int replace_what, int replace_with) { + for(int i=0; i < map_size_rows; i++){ + for(int j=0; j < map_size_cols; j++){ + if(matrix[i][j] == replace_what) matrix[i][j] = replace_with; + } + } +} + +int check_point_boundaries(int point_x, int point_y) { + int result; + if (point_x < map_size_rows && point_x >= 0 && point_y < map_size_cols && point_y >= 0) { + result = 1; //true + } else result = 0; //false + return result; +} + +int get_x(int index) { + return (index - (index % map_size_cols)) / map_size_cols; +} +int get_y(int index) { + return index % map_size_cols; +} + +int get_index(int x, int y) { + int index = x * map_size_cols + y; + return index; +} +int check_point_values(int matrix[map_size_rows][map_size_cols], int point_x, int point_y) { + int point_up_x = point_x; int point_up_y = point_y - 1; + int point_left_x = point_x + 1; int point_left_y = point_y; + int point_right_x = point_x - 1; int point_right_y = point_y; + int point_down_x = point_x; int point_down_y = point_y + 1; + + //check for value not OBST + int point = matrix[point_x][point_y]; + int point_up = matrix[point_up_x][point_up_y]; //up + int point_left = matrix[point_left_x][point_left_y]; //left + int point_right = matrix[point_right_x][point_right_y]; //right + int point_down = matrix[point_down_x][point_down_y]; //down + + int not_checked_value = -4; + int size_v = 4; + static int checked_values[4]; + static int store_indexes[4]; + + for(int i = 0; i < size_v; i++){ + checked_values[i] = not_checked_value; + store_indexes[i] = not_checked_value + 1; + } + + if (check_point_boundaries(point_up_x, point_up_y) && point_up != OBST) { + store_indexes[0] = get_index(point_up_x, point_up_y); + checked_values[0] = point_up; + } + if (check_point_boundaries(point_left_x, point_left_y) && point_left != OBST) { + store_indexes[1] = get_index(point_left_x, point_left_y); + checked_values[1] = point_left; + } + if (check_point_boundaries(point_right_x, point_right_y) && point_right != OBST) { + store_indexes[2] = get_index(point_right_x, point_right_y); + checked_values[2] = point_right; + } + if (check_point_boundaries(point_down_x, point_down_y) && point_down != OBST) { + store_indexes[3] = get_index(point_down_x, point_down_y); + checked_values[3] = point_down; + } + + int to_return = -5; + int ind_up = 2; int ind_l = 0; int ind_r = 3; int ind_d = 1; + //for testing - show up left right down variants + if(test) { + int value_show = 3; + + for(int i = 0; i < value_show; i++){ + for(int j = 0; j < value_show; j++){ + int to_print = 0; + int to_print_index = -1; + + if(i == 0 && j == 1) { + to_print = checked_values[ind_up]; //up + to_print_index = store_indexes[ind_up]; + } + if(i == 1 && j == 0) { + to_print = checked_values[ind_l]; //l + to_print_index = store_indexes[ind_l]; + } + if(i == 1 && j == 1) { + to_print = point; //l + to_print_index = point_x * map_size_cols + point_y; + } + if(i == 1 && j == 2) { + to_print = checked_values[ind_r]; //r + to_print_index = store_indexes[ind_r]; + } + if(i == 2 && j == 1) { + to_print = checked_values[ind_d]; //d + } + if(to_print) { + pr(to_print); pr_s("["); pr(to_print_index); pr_s("] "); + } else { + pr_s(" "); + pr_s(" "); pr_s("--"); pr_s(" "); + } + } ln(); + } + } + + if(test) { + pr(checked_values[ind_up]); pr_s(" ");pr(checked_values[ind_l]);pr_s(" "); pr(checked_values[ind_r]); pr_s(" ");pr(checked_values[ind_d]); + pr_s(" Point_checked_values \n"); //ln(); + } + if(test) { + pr(store_indexes[ind_up]); pr_s(" ");pr(store_indexes[ind_l]);pr_s(" "); pr(store_indexes[ind_r]); pr_s(" ");pr(store_indexes[ind_d]); + pr_s(" : val[Index]\n "); + } + + if(test) pr_s("possible variant(s):\n"); + int minimum_value_index = -1; + int minimum_value = 100; + for(int i = 0; i < size_v; i++){ + int next_point = point - 1; + if(checked_values[i] > 0 && store_indexes[i] >= 0 && checked_values[i] <= next_point && checked_values[i] != OBST){ + int test_ind = store_indexes[i]; + int test_x = get_x(test_ind); + int test_y = get_y(test_ind); + if(checked_values[i] < minimum_value) { + minimum_value = checked_values[i]; + minimum_value_index = store_indexes[i]; + } + if(test) { + pr(checked_values[i]); pr_s("["); pr(store_indexes[i]); pr_s("]="); //pr(i); + pr(ind[test_x][test_y]); pr_s("["); pr(test_x); pr_s(","); pr(test_y); pr_s("]"); + pr_s(",\n"); + } + to_return = minimum_value_index; //TODO: when return which? if sroe_index[i] open - return and close = 1 + } + } + return to_return; +} + +void calc_matrix_wave_func(void) { + ind[start_x][start_x] = start; + insert_obstcls(ind, OBST); + + //fill matrix from 1 to N without obstcls + int exit_flag = 0; + int test_snake = 0; //0 - fill //1 - test off + + if(!test_snake){ + for(int i = start_x; i < map_size_rows; i++) { + for(int j = start_y; j < map_size_cols; j++) { + if(ind[i][j] != OBST) ind[i][j] = i + j + 1; //+start + if(ind[i][j] > path_max_value) path_max_value = ind[i][j]; //goal target? + if(i == goal_x && j == goal_y) { + exit_flag = 1; + break; + } + } + if(exit_flag) break; + } + } + pr_s("SHOW IND"); ln(); + show_matrix(ind); + + int filling_path_with = 1; + int point_down = goal_x; int point_right = goal_y; // int point_down = 7; int point_right = 3; + if(test_main_path) { + pr_s(" FROM {|x = "); pr(point_down); + pr_s(", -y ="); pr(point_right); + pr_s("}"); ln(); + } + int returned_index = check_point_values(ind, point_down, point_right); //return one index to go + int next_x = get_x(returned_index); + int next_y = get_y(returned_index); + + if(test_main_path) { + pr_s("point "); pr(ind[point_down][point_right]); + } + if(test_main_path) { + pr_s(" next |x:"); pr(next_x); pr_s(", -y:");pr(next_y); + } + + int value = ind[next_x][next_y]; + if(test_main_path) { + pr_s(" "); pr(value); pr_s("["); pr(returned_index); pr_s("] !="); pr(start); ln(); + } + int path_found = 1; + int exit_path_not_found = map_size_rows * map_size_cols + 1; + while(1){ + if (next_y == start_y && next_x == start_x) break; + point_down = next_x; point_right = next_y; //get new position + update_matrix_with(path, point_down, point_right, filling_path_with); //encoding path + if(test) show_matrix(path); + if(test_main) { + pr_s("poit_down = "); pr(point_down); + pr_s(", point_right ="); pr(point_right); ln(); + } + returned_index = check_point_values(ind, point_down, point_right); //return one index to go + next_x = get_x(returned_index); + next_y = get_y(returned_index); + if(test_main_path) { + pr_s(" point "); pr(ind[point_down][point_right]); + } + if(test_main_path) { + pr_s(" next x:"); pr(next_x); + pr_s(", next y:"); pr(next_y); + } + value = ind[next_x][next_y]; + if(test_main_path) { + pr_s(" "); pr(value); + pr_s("["); pr(returned_index); pr_s("] !="); + pr(start); + ln(); + } + exit_path_not_found--; + if(exit_path_not_found == 0) { + path_found = 0; //path not found + break; + } + } + point_down = next_x; point_right = next_y; //get new position + update_matrix_with(path, point_down, point_right, filling_path_with); //encoding path + if(test_main) { + pr_s("poit_down = "); pr(point_down); + pr_s(", point_right ="); pr(point_right); ln(); + } + returned_index = check_point_values(ind, point_down, point_right); //return one index to go + next_x = get_x(returned_index); + next_y = get_y(returned_index); + if(test_main_path) { + pr_s(" point "); pr(ind[point_down][point_right]); + } + if(test_main_path) { + pr_s(" next x:"); pr(next_x); pr_s(" != "); pr(start_y); + pr_s(", next y:"); pr(next_y); pr_s(" != "); pr(start_x); + } + value = ind[next_x][next_y]; + if(test_main_path) { + pr_s(" "); pr(value); + pr_s("["); pr(returned_index); pr_s("] !="); + pr(start); + ln(); + } + + start = filling_path_with; + update_matrix_with(path, start_x, start_y, start); + update_matrix_with(path, goal_x, goal_y, goal_target); //for -1 +// update_matrix_with(path, goal_x, goal_y, filling_path_with); //for 0 + + if(path_found) { + //final encoding + replace_all(path, 1, 2); + replace_all(path, 0, 1); + replace_all(path, 2, 0); //path 0 + pr_s("Path found\n"); +// show_matrix(path); + } else { + //TODO: path not found what todo? go around??)) dance + pr_s("Path NOT found\n"); ln(); + } + calc_matrix_wave = 0; //calculated the wave, got one path +} // path part finish + +void setup() { + //Ulterasonic + //Serial Port begin + Serial.begin(9600); + //Define inputs and outputs pins for ultrasonic + pinMode(trigPin, OUTPUT); + pinMode(echoPin, INPUT); + + // Set all the motor control pins to outputs + pinMode(enA, OUTPUT); + pinMode(enB, OUTPUT); + pinMode(in1, OUTPUT); + pinMode(in2, OUTPUT); + pinMode(in3, OUTPUT); + pinMode(in4, OUTPUT); + +} + + +/** + turns on the motors to go forward and calculate the current indexes after moving. + + @param None. + @return None. +*/ + +void forward() { + + // Turn on motor B and A to go forward + digitalWrite(in1, HIGH); + digitalWrite(in2, LOW); + digitalWrite(in3, HIGH); + digitalWrite(in4, LOW); + + // Set the speed value out of possible range 0~255 + analogWrite(enB, 200); + analogWrite(enA, 182); + delay(1800); + + Serial.println("Forward"); + + //Change the indexs on the path matrix after the robot move + if (direction == 0) + j = j - 1; + if (direction == 1) + i = i + 1; + if (direction == 2) + j = j + 1; + if (direction == 3) + i = i - 1; +} + + +/** + turns on the motors to turn to the left and change the current direction after moving. + + @param None. + @return None. +*/ + +void left() { + + // Turn on motor B and A to turn to the left + digitalWrite(in1, HIGH); + digitalWrite(in2, LOW); + digitalWrite(in3, LOW); + digitalWrite(in4, HIGH); + + delay(170); + Serial.println("Left"); + + // Change the direction of the robot after it turn left + if (direction == 0) + direction = 3; + else if (direction == 1) + direction = 0; + else if (direction == 2) + direction = 1; + else if (direction == 3) + direction = 2; + +} + +/** + turns on the motors to turn to the right and change the current direction after moving. + + @param None. + @return None. +*/ + +void right() { + + // Turn on motor B and A to turn to the right + digitalWrite(in1, LOW); + digitalWrite(in2, HIGH); + digitalWrite(in3, HIGH); + digitalWrite(in4, LOW); + //analogWrite(enB, 200); + // analogWrite(enA, 200); + delay(200); + Serial.println("Right"); + + if (direction == 0) + direction = 1; + else if (direction == 1) + direction = 2; + else if (direction == 2) + direction = 3; + else if (direction == 3) + direction = 0; +} + +/** + turns off the motors for 2 secondes. + + @param None. + @return None. +*/ +void stopRobot() { + if (test_arduino) pr_s("stop_robot"); ln(); + //Stop the motors + digitalWrite(in1, LOW); + digitalWrite(in2, LOW); + digitalWrite(in3, LOW); + digitalWrite(in4, LOW); + delay(2000); +} + +/** + turns on the motors to turn 180 degree and change the current direction after moving. + + @param None. + @return None. +*/ +void turn180() { + + Serial.println("Around"); + digitalWrite(in1, HIGH); + digitalWrite(in2, LOW); + digitalWrite(in3, LOW); + digitalWrite(in4, HIGH); + analogWrite(enB, 200); + analogWrite(enA, 175); + delay(500); + if (direction == 0) + direction = 2; + else if (direction == 1) + direction = 3; + else if (direction == 2) + direction = 0; + else if (direction == 3) + direction = 1; + + +} +/** + Calculate the distance in the front of the robot. + + @param None. + @return a the distance in cm. +*/ +long calcDistance() { + + // The sensor is triggered by a HIGH pulse of 10 or more microseconds. + // Give a short LOW pulse beforehand to ensure a clean HIGH pulse: + digitalWrite(trigPin, LOW); + delayMicroseconds(5); + digitalWrite(trigPin, HIGH); + delayMicroseconds(10); + digitalWrite(trigPin, LOW); + + // Read the signal from the sensor: a HIGH pulse whose + // duration is the time (in microseconds) from the sending + // of the ping to the reception of its echo off of an object. + pinMode(echoPin, INPUT); + duration = pulseIn(echoPin, HIGH); + + // Convert the time into a distance + cm = (duration / 2) / 29.1; // Divide by 29.1 or multiply by 0.0343 + + Serial.print(cm); + Serial.print("cm"); + Serial.println(); + delay(300); + + return cm; +} +/** + calulate the J index of the obstecal. + + @param None. + @return the value of J index . +*/ +int returnIndexJ() { + if (direction == 0) { + return j - 1; + } + if (direction == 1 || direction == 3) { + return j; + } + if (direction == 2) { + return j + 1; + } +} + +/** + calulate the i index of the obstecal. + + @param None. + @return the value of i index . +*/ +int returnIndexI() { + if (direction == 0 || direction == 2) { + return i; + } + if (direction == 1) { + return i + 1; + } + + if (direction == 3) { + return i - 1; + } +} + +/** + test front of the robot due to the path matrix depending on the direction. + + @param None. + @return the value of the matrix for the front. +*/ +int testFront() { + if (direction == 0) { + return path[j - 1][i]; + } + if (direction == 1) { + return path[j][i + 1]; + } + if (direction == 2) { + return path[j + 1][i]; + } + if (direction == 3) { + return path[j][i - 1]; + } +} + + +/** + test the right of the robot due to the path matrix depending on the direction. + + @param None. + @return the value of the matrix to the right. +*/ +int testRight() { + if (direction == 0) { + return path[j][i + 1]; + } + if (direction == 1) { + return path[j + 1][i]; + } + if (direction == 2) { + return path[j][i - 1]; + } + if (direction == 3) { + return path[j - 1][i]; + } +} + + + +/** + test the left of the robot due to the path matrix depending on the direction. + + @param None. + @return the value of the matrix to the left. +*/ +int testLeft() { + if (direction == 0) { + return path[j][i - 1]; + } + if (direction == 1) { + return path[j - 1][i]; + } + if (direction == 2) { + return path[j][i + 1]; + } + if (direction == 3) { + return path[j + 1][i]; + } +} + + +/** + return the value of the matrix so we can now if we can go oe not. + + @param None. + @return 1 if the front of the robot is closed due to the grid and the value of the grid if the front is open(0 or -1). +*/ +int frontOpen() { + if (test_arduino) pr_s("frontOpen"); ln(); + int next = testFront(); + if (next == 0) { + return next; + } + else if (next == -1) { + goal = 1; + return next; + } + + else { + return 1; + } +} + +/** + test if the right is open to turn to it or not. + + @param None. + @return True or False. +*/ +boolean rightOpen() { + if (test_arduino) pr_s("rightOpen"); ln(); + int next = testRight(); + if (next == 0 || next == -1) { + return true; + } + else { + return false; + } +} + +/** + test if the left is open to turn to it or not. + + @param None. + @return True or False. +*/ +boolean leftOpen() { + int next = testLeft(); + if (next == 0 || next == -1) { + return true; + } + else { + return false; + } +} +void set_obstecal() { + int y = (map_size_rows - 1) - returnIndexI(); + int x = (map_size_cols - 1) - returnIndexJ(); + pr_s("x=");pr(x);pr_s("y="); pr(y); ln(); + ind[y][x] = OBST; + pr_s("!! = "); pr(ind[y][x]); ln(); +// update_matrix_with(ind, y, x, OBST); + pr_s("New ind"); ln(); + show_matrix(ind); +} + +void clean_path(){ + for(int i = 0; i < map_size_rows; i++){ + for(int j = 0; j < map_size_cols; j++){ + path[i][j] = 0; + + } + } + +} + +void loop() { + pr_s("HELLO"); + if(calc_matrix_wave) { + calc_matrix_wave_func(); //return path +// pr_s("PATH"); ln(); + show_matrix(path); + } + if (calcDistance() < 35) { + calc_matrix_wave = 1; // we can recalc path + stopRobot(); + clean_path(); +// set_obstecal(); + int y = (map_size_rows - 1) - return_indexI(); + int x = (map_size_cols - 1) - return_indexJ(); + pr_s("x=");pr(x);pr_s("y="); pr(y); ln(); + ind[y][x] = OBST; + pr_s("!! = "); pr(ind[y][x]); ln(); +// update_matrix_with(ind, y, x, OBST); + pr_s("New ind"); ln(); + show_matrix(ind); + + +// pr_s("New ind"); ln(); +// show_matrix(ind); + pr_s("-------New ind"); ln(); + +// left(); +// +// if (rightOpen() == true) { +// right(); +// } +// else if (leftOpen() == true) { +// left(); +// } +// else { +// turn180(); +// } +} + else { +// pr_s("PATH"); ln(); +// show_matrix(path); + //delay(400); + if (frontOpen() == 0 || frontOpen() == -1) { + forward(); + stop_robot(); + //delay(1000); + if (goal==1) { + stop_robot(); + delay(100000); + } + } + else if (rightOpen() == true) { + right(); + } + else if (leftOpen() == true) { + left(); + } + else { + turn180(); + + } + } +} + + + diff --git a/src/main.c b/src/main.c new file mode 100644 index 0000000..2ee0861 --- /dev/null +++ b/src/main.c @@ -0,0 +1,251 @@ +#include + +#define map_size_rows 8 +#define map_size_cols 8 +#define OBST 99 +//beautiful output +#define beautiful_output 1 + +int ind[map_size_rows][map_size_cols] = { + {0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0} +}; + +int path[map_size_rows][map_size_cols] = { + {0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0} +}; + +int update_matrix_with(int matrix[map_size_rows][map_size_cols], int x, int y, int new_value){ + //check if position exist + if(x >=0 && x <= map_size_rows && y >= 0 && y <= map_size_cols) + matrix[x][y] = new_value; + else + printf("Can't update, position doesn't exist\n"); + return matrix[x][y]; +} + +void insert_obstcl(int ind[map_size_rows][map_size_cols], int obst){ +// update_matrix_with(ind, 1, 1, OBST); + update_matrix_with(ind, 3, 4, OBST); +} + +void ind_first_line(void) { + printf(".. "); + for(int i = 0; i < map_size_rows; i++) + printf(" %2d",i); + printf("\n"); +} + +void show_matrix(int matrix[map_size_rows][map_size_cols]){ + if(beautiful_output) ind_first_line(); + for(int i=0; i < map_size_rows; i++){ + if(beautiful_output) printf("%2d: ",i); + for(int j=0; j < map_size_cols; j++){ + printf("%2d ",matrix[i][j]); + } + printf("\n"); + } printf("\n"); +} + +void show_matrix_revert(int matrix[map_size_rows][map_size_cols]){ + if(beautiful_output) { + printf(".. "); + for(int i = map_size_rows - 1; i >= 0; i--) + printf(" %2d",i); + printf("\n"); + } + for(int i = map_size_rows - 1; i >= 0; i--) { + if(beautiful_output) printf("%2d: ",i); + for(int j = map_size_rows - 1; j >= 0; j--) { + printf("%2d ",matrix[i][j]); + } + printf("\n"); + } printf("\n"); +} + +void replace_all(int matrix[map_size_rows][map_size_cols], int replace_what, int replace_with) { + for(int i=0; i < map_size_rows; i++){ + for(int j=0; j < map_size_cols; j++){ + if(matrix[i][j] == replace_what) matrix[i][j] = replace_with; + } + } +} + +void pr(int value) { + printf("%2d ", value); +} + +int check_point_boundaries(int point_x, int point_y) { + int result; + if (point_x < map_size_rows && point_x >= 0 && point_y < map_size_cols && point_y >= 0) { + result = 1; //true + } else result = 0; //false + return result; +} + +int get_x(int index){ + return index % map_size_rows; +} + +int get_y(int index){ + return (index - (index % map_size_rows)) / map_size_rows ; +} + +int check_point_values(int matrix[map_size_rows][map_size_cols], int point_x, int point_y) { + int test = 0; //beautiful output + + int point_up_x = point_x; int point_up_y = point_y + 1; + int point_left_x = point_x - 1; int point_left_y = point_y; + int point_right_x = point_x + 1; int point_right_y = point_y; + int point_down_x = point_x; int point_down_y = point_y - 1; + + //check for value not OBST + int point = matrix[point_x][point_y]; + int point_up = matrix[point_up_x][point_up_y]; //up + int point_left = matrix[point_left_x][point_left_y]; //left + int point_right = matrix[point_right_x][point_right_y]; //right + int point_down = matrix[point_down_x][point_down_y]; //down + + int not_checked_value = -4; + int checked_point_up = not_checked_value; + int checked_point_left = not_checked_value; + int checked_point_right = not_checked_value; + int checked_point_down = not_checked_value; + + //check x,y in 4 directions for barrer boundaries + if (check_point_boundaries(point_up_x, point_up_y) && point_up != OBST) checked_point_up = point_up; + if (check_point_boundaries(point_left_x, point_left_y) && point_left != OBST) checked_point_left = point_left; + if (check_point_boundaries(point_down_x, point_down_y) && point_right != OBST) checked_point_down = point_down; + if (check_point_boundaries(point_right_x, point_right_y) && point_down != OBST) checked_point_right = point_right; + + int size_v = 4; + static int checked_values[4]; + static int store_indexes[4]; + + int filling_value = -3; //just to fill with something + + for(int i = 0; i < size_v; i++) { + checked_values[i] = filling_value; + store_indexes[i] = filling_value + 1; + } + + int map_size = map_size_rows * map_size_cols; + int test_sum = point_up_x * map_size_rows + point_up_y; + if(test_sum < map_size) store_indexes[0] = test_sum; + + test_sum = point_left_x * map_size_rows + point_left_y; + if(test_sum < map_size) store_indexes[1] = test_sum; + + test_sum = point_right_x * map_size_rows + point_right_y; + if(test_sum < map_size) store_indexes[2] = test_sum; + + test_sum = point_down_x * map_size_rows + point_down_y; + if(test_sum < map_size) store_indexes[3] = test_sum; + + if(test) printf("%2d %2d %2d %2d Point_checked_values\n", checked_point_up, checked_point_left, checked_point_right, checked_point_down); + + if(test) { + for(int i=0; i < size_v; i++){ + int index = store_indexes[i]; + printf("%2d ", index); + } + printf(" :val[Index]\n "); + } + + checked_values[0] = checked_point_up; + checked_values[1] = checked_point_left; + checked_values[2] = checked_point_right; + checked_values[3] = checked_point_down; + + int to_return = -5; + if(test) printf("possible variant(s):"); + for(int i = 0; i < size_v; i++){ + if(checked_values[i] > 0 && store_indexes[i] >= 0 && checked_values[i] < point && checked_values[i] != OBST){ + if(test) printf("%2d[%d]: %d, ", checked_values[i], store_indexes[i], i); + to_return = store_indexes[i]; + } + } + if(test)printf("\n"); + return to_return; +} + +int main() { + int test_main_display = 1; + int test_main_path = 1; + int start = 1; //start point + int start_x = 0; //vertical + int start_y = 0; //horisontal + ind[start_x][start_y] = start; + + int goal_target = -1; + int goal_x = map_size_cols - 1; int goal_y = map_size_rows - 1; +// int goal_x = 3; int goal_y = 7; + + ind[goal_x][goal_y] = goal_target; + update_matrix_with(path, goal_x, goal_y, goal_target); + + insert_obstcl(ind, OBST); + int path_max_value = 1; + + //fill matrix from 1 to goal + for(int i = 0; i < map_size_rows; i++) { + for(int j = 0; j < map_size_cols; j++) { + //TODO: stop if we met goal + //if(ind[i][j] != goal_target) + if(ind[i][j] != OBST) ind[i][j] = i+j+1; + if(ind[i][j] > path_max_value) path_max_value = ind[i][j]; + } + } + + if(test_main_display) show_matrix(ind); + + //path finding + /* + from goal to start we can use any way... + go and show x,y, number + */ + + int point_down = goal_x; int point_right = goal_y; + + if(test_main_path) printf(" FROM x horisontal = %d, vertical y = %d \n", point_down, point_right); + + int returned_index = check_point_values(ind, point_down, point_right); //return one index to go + int next_x = get_x(returned_index); + int next_y = get_y(returned_index); + + if(test_main_path) printf("point%2d,",ind[point_down][point_right]); + if(test_main_path) printf(" next x:%d, y:%d ", next_x, next_y); + int value = ind[next_x][next_y]; + if(test_main_path) printf("%2d[%d]\n", value, returned_index); + + int filling_path_with = 1; +// for(int count = 0; count < 1; count++){ + while(value != start){ + point_down = next_y; point_right = next_x; //get new position +// path[point_down][point_right] = filling_path_with; + update_matrix_with(path, point_down, point_right, filling_path_with); //encoding path +// if(test_main) printf(" poit_down = %d, poit_down = %d \n", point_down, point_right); + returned_index = check_point_values(ind, point_down, point_right); //return one index to go + next_x = get_x(returned_index); + next_y = get_y(returned_index); + if(test_main_path) printf("point%2d,",ind[point_down][point_right]); + if(test_main_path) printf(" next x:%d, y:%d ", next_x, next_y); + value = ind[next_x][next_y]; + if(test_main_path) printf("%2d[%d]\n", value, returned_index); + } + + return 0; +} diff --git a/src/movment_with_millis/movment_with_millis.ino b/src/movment_with_millis/movment_with_millis.ino new file mode 100644 index 0000000..c749e18 --- /dev/null +++ b/src/movment_with_millis/movment_with_millis.ino @@ -0,0 +1,424 @@ + +int enA = 9; +int in1 = 8; +int in2 = 7; + +// Motor B + +int enB = 3; +int in3 = 5; +int in4 = 4; + + +//Ultrasonic +int trigPin = 11; // Trigger +int echoPin = 10; // Echo +long duration, cm; +// 0 is up +// 1 is right +// 2 is down +// 3 is left +int direction = 2; +// the point in the grid where the robot is + +int i = 2; +int j = 0; + +int path[8][8] = +{ + { 1,0,0,1,1,1,1,1 } + , + { 1,1,0,1,1,1,1,1 } + , + { 1,1,0,1,1,1,1,1 } + , + { 1,1,0,1,1,1,1,1 } + , + { 1,1,0,0,1,1,1,1 } + , + { 1,1,1,-1,1,1,1,1 } +}; + +unsigned long currentMillis = millis(); +unsigned long previousMillis = 0; +bool goal = false; + +void setup() + +{ + //Ulterasonic + //Serial Port begin + Serial.begin(9600); + //Define inputs and outputs pins for ultrasonic + pinMode(trigPin, OUTPUT); + pinMode(echoPin, INPUT); + + // Set all the motor control pins to outputs + + pinMode(enA, OUTPUT); + pinMode(enB, OUTPUT); + pinMode(in1, OUTPUT); + pinMode(in2, OUTPUT); + pinMode(in3, OUTPUT); + pinMode(in4, OUTPUT); + +} + +/** + turns on the motors to go forward. + + @param None. + @return None. +*/ + +void forward() { + currentMillis = millis(); + Serial.println(currentMillis); + // Turn on motor B and A to go forward + digitalWrite(in1, HIGH); + digitalWrite(in2, LOW); + digitalWrite(in3, HIGH); + digitalWrite(in4, LOW); + + analogWrite(enB, 200); + analogWrite(enA, 175); + + if (currentMillis - previousMillis >= 1800) { + + Serial.println("Forward"); + digitalWrite(in1, LOW); + digitalWrite(in2, LOW); + digitalWrite(in3, LOW); + digitalWrite(in4, LOW); + + //Change the indexs on the path matrix after the robot move + if (direction == 0) + j = j - 1; + if (direction == 1) + i = i + 1; + if (direction == 2) + j = j + 1; + if (direction == 3) + i = i - 1; + previousMillis = currentMillis; + if (goal) { + + delay(100000); + } + + } +} + +/** + turns on the motors to turn to the left. + + @param None. + @return None. +*/ +void left() { + currentMillis = millis(); +Serial.println(currentMillis); + // Turn on motor B and A to turn to the left + digitalWrite(in1, HIGH); + digitalWrite(in2, LOW); + digitalWrite(in3, LOW); + digitalWrite(in4, HIGH); + analogWrite(enB, 200); + analogWrite(enA, 175); + + + if (currentMillis - previousMillis >= 500) { + + Serial.println("Left"); + digitalWrite(in1, LOW); + digitalWrite(in2, LOW); + digitalWrite(in3, LOW); + digitalWrite(in4, LOW); + // Change the direction of the robot after it turn left + if (direction == 0) + direction = 3; + else if (direction == 1) + direction = 0; + else if (direction == 2) + direction = 1; + else if (direction == 3) + direction = 2; + previousMillis = currentMillis; + + } +} +/** + turns on the motors to turn to the right. + + @param None. + @return None. +*/ +void right() { + currentMillis = millis(); +Serial.println(currentMillis); + // Turn on motor B and A to turn to the right + digitalWrite(in1, LOW); + digitalWrite(in2, HIGH); + digitalWrite(in3, HIGH); + digitalWrite(in4, LOW); + analogWrite(enB, 200); + analogWrite(enA, 175); + + + if (currentMillis - previousMillis >= 500) { + Serial.println("Right"); + digitalWrite(in1, LOW); + digitalWrite(in2, LOW); + digitalWrite(in3, LOW); + digitalWrite(in4, LOW); + + if (direction == 0) + direction = 1; + else if (direction == 1) + direction = 2; + else if (direction == 2) + direction = 3; + else if (direction == 3) + direction = 0; + previousMillis = currentMillis; + } +} +/** + turns on the motors to turn 180 degree. + + @param None. + @return None. +*/ +void turn180() { + currentMillis = millis(); + Serial.println("Around"); + digitalWrite(in1, HIGH); + digitalWrite(in2, LOW); + digitalWrite(in3, LOW); + digitalWrite(in4, HIGH); + + if (currentMillis - previousMillis >= 1000) { + digitalWrite(in1, LOW); + digitalWrite(in2, LOW); + digitalWrite(in3, LOW); + digitalWrite(in4, LOW); + if (direction == 0) + direction = 2; + else if (direction == 1) + direction = 3; + else if (direction == 2) + direction = 0; + else if (direction == 3) + direction = 1; + previousMillis = currentMillis; + } + + +} + +/** + turns ofthe motors to stop. + + @param None. + @return None. +*/ +void stop_robot() { + //Stop the motors + digitalWrite(in1, LOW); + digitalWrite(in2, LOW); + digitalWrite(in3, LOW); + digitalWrite(in4, LOW); + delay(2000); +} + +/** + Calculate the distance in the front of the robot. + + @param None. + @return a the distance in cm. +*/ +long calcDistance() {/////most change the comments--------------------------------- + + // The sensor is triggered by a HIGH pulse of 10 or more microseconds. + // Give a short LOW pulse beforehand to ensure a clean HIGH pulse: + digitalWrite(trigPin, LOW); + delayMicroseconds(5); + digitalWrite(trigPin, HIGH); + delayMicroseconds(10); + digitalWrite(trigPin, LOW); + + // Read the signal from the sensor: a HIGH pulse whose + // duration is the time (in microseconds) from the sending + // of the ping to the reception of its echo off of an object. + pinMode(echoPin, INPUT); + duration = pulseIn(echoPin, HIGH); + + // Convert the time into a distance + cm = (duration / 2) / 29.1; // Divide by 29.1 or multiply by 0.0343 + + Serial.print(cm); + Serial.print("cm"); + Serial.println(); + // delay(250); + + return cm; +} + + + +/** + test front of the robot due to the path matrix depending on the direction. + + @param None. + @return the value of the matrix for the front. +*/ +int test_front() { + if (direction == 0) { + return path[j - 1][i]; + } + if (direction == 1) { + return path[j][i + 1]; + } + if (direction == 2) { + return path[j + 1][i]; + } + if (direction == 3) { + return path[j][i - 1]; + } +} + +/** + test right of the robot due to the path matrix depending on the direction. + + @param None. + @return the value of the matrix for the right. +*/ +int test_right() { + if (direction == 0) { + return path[j][i + 1]; + } + if (direction == 1) { + return path[j + 1][i]; + } + if (direction == 2) { + return path[j][i - 1]; + } + if (direction == 3) { + return path[j - 1][i]; + } +} + +/** + test left of the robot due to the path matrix depending on the direction. + + @param None. + @return the value of the matrix for the left. +*/ +int test_left() { + if (direction == 0) { + return path[j][i - 1]; + } + if (direction == 1) { + return path[j - 1][i]; + } + if (direction == 2) { + return path[j][i + 1]; + } + if (direction == 3) { + return path[j + 1][i]; + } +} + + +/** + return the value of the matrix so we can now if we can go oe not. + + @param None. + @return 1 if the front of the robot is closed due to the grid and the value of the grid if the front is open(0 or -1) +*/ +int frontOpen() { + int next = test_front(); + if (next == 0) { + return next; + } + else if (next == -1) { + goal = true; + return next; + } + + else { + return 1; + } +} + +/** + test if the right is open to turn to it or not + + @param None. + @return return True or False. +*/ +boolean rightOpen() { + int next = test_right(); + if (next == 0 || next == -1) { + return true; + } + else { + return false; + } +} +/** + test if the left if is open to turn to it or not + + @param None. + @return return True or False. +*/ +boolean leftOpen() { + int next = test_left(); + if (next == 0 || next == -1) { + return true; + } + else { + return false; + } +} + +void set_obstecal() { + path[i][j] = 1; + //recalc +} + +void loop() { + if (calcDistance() < 35) { + stop_robot(); + set_obstecal(); + if (rightOpen() == true) { + right(); + } + else if (leftOpen() == true) { + left(); + } + else { + turn180();} + //recalc path---------------------------------------------- + } + else { + + if (frontOpen() == 0 || frontOpen() == -1) { + forward(); + + } + else if (rightOpen() == true) { + right(); + } + else if (leftOpen() == true) { + left(); + } + else { + turn180(); + } + } +} + + + +