astarbase.c

#pragma config(Sensor, S1,     MSNP,           sensorI2CCustomFastSkipStates)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/**
*Michael Annor and Agatha Maison
*A* Implementation for Robotics Final Project 
*Ashesi Fall 2015
**/

#include "planning_map_base.h"
#include "drivers/mindsensors-numericpad.h"

//properties of a cell
typedef struct{
	int x;//ij
	int y;//jj
	int val;//map
	int f; int g; int h;//g(0),h(calc), f(0)
	int parentx;//-1
	int parenty;//-1
	bool onopen;//f
	bool onclosed;//f
} node;


//creating a 2 dimensional node map
node node_map[Y_SIZE][X_SIZE];

//node array to track neighbours
node current_neighbours[CONNECTIVITY];

//open list
node open[MAP_AREA];
int front;
int rear;

//represents the current location of the passenger (goal for any given taxi)
int goal_x; int goal_y;


//function to set h cost for a given cell
int set_h_cost(int x, int y){
	int h;
	int dx;
	int dy;
	dx=goal_x-x;
	dy=goal_y-y;
	h = sqrt((dx*dx)+(dy*dy))* VH_COST;
	h = abs(h);
	return h;
}

//location and destination coordinates of a passenger
int passenger_location_x;
int passenger_location_y;
int passenger_destination_x;
int passenger_destination_y;

//function to get passenger's current x location from numeric keypad
int get_passenger_location_x(){

	int keys = 0;
	unsigned byte key[] = {0};
	int number = 0;
	string output;

	eraseDisplay();
	displayCenteredTextLine(0, "Enter x-location");
	displayTextLine(1, "-------------------");
	displayTextLine(5, "-------------------");

	while (true){
		if (!MSNPscanKeys(MSNP, keys, key[0], number))
			stopAllTasks();

		if ((number>=0)&&(number<20)){
			//	if (number == 8){number = 0;}
			displayTextLine(3, "Numpad Key: %d", number);
			wait1Msec(1000);
			sleep(100);
			return number;
			break;
		}
	}
}

//function to get passenger's current y location from numeric keypad
int get_passenger_location_y(){

	int keys = 0;
	unsigned byte key[] = {0};
	int number = 0;
	string output;

	eraseDisplay();
	displayCenteredTextLine(0, "Enter y-location");
	displayTextLine(1, "-------------------");
	displayTextLine(5, "-------------------");

	while (true){
		if (!MSNPscanKeys(MSNP, keys, key[0], number))
			stopAllTasks();

		if ((number>=0)&&(number<20)){
			//if (number == 8){number = 0;}
			displayTextLine(3, "Numpad Key: %d", number);
			wait1Msec(1000);
			sleep(100);
			return number;
			break;
		}

	}
}

//function to get passenger's x coordinate for his destination from numeric keypad
int get_passenger_destination_x(){

	int keys = 0;
	unsigned byte key[] = {0};
	int number = 0;
	string output;

	eraseDisplay();
	displayCenteredTextLine(0, "Enter x-destination");
	displayTextLine(1, "-------------------");
	displayTextLine(5, "-------------------");

	while (true){
		if (!MSNPscanKeys(MSNP, keys, key[0], number))
			stopAllTasks();

		if ((number>=0)&&(number<20)){
			//	if (number == 8){number = 0;}
			displayTextLine(3, "Numpad Key: %d", number);
			wait1Msec(1000);
			sleep(100);
			return number;
			break;
		}
	}
}

//function to get passenger's y coordinate for his destination from numeric keypad
int get_passenger_destination_y(){

	int keys = 0;
	unsigned byte key[] = {0};
	int number = 0;
	string output;

	eraseDisplay();
	displayCenteredTextLine(0, "Enter y-destination");
	displayTextLine(1, "-------------------");
	displayTextLine(5, "-------------------");

	while (true){
		if (!MSNPscanKeys(MSNP, keys, key[0], number))
			stopAllTasks();



		if ((number>=0)&&(number<20)){
			//if (number == 8){number = 0;}
			displayTextLine(3, "Numpad Key: %d", number);
			wait1Msec(1000);
			sleep(100);
			return number;
			break;
		}

	}
}

//function to store values read from the keypad into assigned variables
void base_station(){
	passenger_location_x=get_passenger_location_x();
	passenger_location_y=get_passenger_location_y();
	passenger_destination_x=get_passenger_destination_x();
	passenger_destination_y=get_passenger_destination_y();
}

//function to initialize the node/cell with all the properties a cell should have
void init_node(node *cell){
	cell->val=world_map[cell->y][cell->x];
	cell->f=0;
	cell->g=0;
	cell->h=set_h_cost(cell->x,cell->y);
	cell->parentx=-1;
	cell->parenty=-1;
	cell->onopen=false;
	cell->onclosed=false;
}

//function to initialize the whole node map
void init_map(){
	for (int i=0; i<Y_SIZE; i++){
		for (int j=0; j<X_SIZE; j++){
			node_map[i][j].x=j;
			node_map[i][j].y=i;
			init_node(&node_map[i][j]);
		}
	}
}

//funciton to intialize open list
void createopen(){
	front = rear = -1;
}

//function to add a node to the open list by priority
void insert_by_priority(node newNode){
	if(rear >= MAP_AREA - 1){//list full
		return;
	}
	newNode.onopen=true;
	if((front==-1)&&(rear==-1)){//first entry
		front++;
		rear++;

		memcpy(&open[rear], &newNode, sizeof(open[rear]));
		node_map[newNode.y][newNode.x].onopen=true;
		return;
	}
	else{
		for(int i=0; i<=rear; i++){
			if(newNode.f <= open[i].f){
				for(int j=rear+1; j>i; j--){
					memcpy(&open[j], &open[j-1], sizeof(open[j]));
				}
				rear++;
				memcpy(&open[i], &newNode, sizeof(open[i]));
				node_map[newNode.y][newNode.x].onopen=true;
				return;
			}
		}
		rear++;
		memcpy(&open[rear], &newNode, sizeof(open[rear]));
		node_map[newNode.y][newNode.x].onopen=true;
	}
}

//function to get the node with the lowest f cost from the open list
void get_lowest_f_cost(int &lowx, int &lowy){
	//remove function from the priority queue
	int i;
	if ((front==-1) && (rear==-1)){
		//Queue is empty no elements to remove
		return;
	}
	lowx=open[0].x;
	lowy=open[0].y;
	node_map[lowy][lowx].onopen=false;//remove from open
	for (i = 0; i <= rear; i++){
		memcpy(open[i], &open[i + 1], sizeof(open[i]));
	}
	rear--;

	if (rear == -1){
		front = -1;
	}
}

//function to add a node to the close list (i.e. sets its onclosed attribute to true)
void add_to_closed(int x, int y){
	node_map[y][x].onclosed=true;
}

//function to check if a given node is the goal node
bool is_goal(int x, int y, int goal_x, int goal_y){
	if(((x==goal_x) && (y==goal_y))!=true){
		return false;
	}
	return true;
}

//function to set the g anf f cost of a given cell(cx,cy). This is relation to the parent (x,y) of the given cell
void set_cost(int cx, int cy, int x, int y){
	if ((cx != x) && (cy != y)){
		node_map[cy][cx].g = node_map[y][x].g+DIAGONAL_COST;
		node_map[cy][cx].f=node_map[cy][cx].g + node_map[cy][cx].h;
	}
	else {
		node_map[cy][cx].g = node_map[y][x].g+VH_COST;
		node_map[cy][cx].f=node_map[cy][cx].g + node_map[cy][cx].h;
	}
}

//function to get the neighbours of a given cell (north, north-east, east, south-east, south, south-west, west, north-west)
void get_neighbours(int x, int y){
	//north
	if (((y-1)<Y_SIZE) && ((y-1)>=0) && ((x)<X_SIZE) && ((x)>=0)){ //checks if node is out of bounds/ out of the world
		if(node_map[y-1][x].g==0){ //checks if g cost has not been set. if not, set it.
			set_cost(node_map[y-1][x].x,node_map[y-1][x].y,x,y);
		}
		memcpy(&current_neighbours[0], &node_map[y-1][x], sizeof(current_neighbours[0]));//copy cell from neighbours array into node map
	}
	else {
		current_neighbours[0].val=1; //else neighbour is an obstacle so set its value attribute to 1
	}

	if (((y)<Y_SIZE) && ((y)>=0) && ((x+1)<X_SIZE) && ((x+1)>=0)){
		if(node_map[y][x+1].g==0){
			set_cost(node_map[y][x+1].x,node_map[y][x+1].y,x,y);
		}
		memcpy(&current_neighbours[2], &node_map[y][x+1], sizeof(current_neighbours[2]));//east
	}
	else {
		current_neighbours[2].val=1;
	}
	if (((y+1)<Y_SIZE) && ((y+1)>=0) && ((x)<X_SIZE) && ((x)>=0)){
		if(node_map[y+1][x].g==0){
			set_cost(node_map[y+1][x].x,node_map[y+1][x].y,x,y);
		}
		memcpy(&current_neighbours[4], &node_map[y+1][x], sizeof(current_neighbours[4]));//south
	}
	else {
		current_neighbours[4].val=1;
	}
	if (((y)<Y_SIZE) && ((y)>=0) && ((x-1)<X_SIZE) && ((x-1)>=0)){
		if(node_map[y][x-1].g==0){
			set_cost(node_map[y][x-1].x,node_map[y][x-1].y,x,y);
		}
		memcpy(&current_neighbours[6], &node_map[y][x-1], sizeof(current_neighbours[6]));//west
	}
	else {
		current_neighbours[6].val=1;
	}
	if (((y-1)<Y_SIZE) && ((y-1)>=0) && ((x+1)<X_SIZE) && ((x+1)>=0)&&(current_neighbours[0].val==FREE)&&(current_neighbours[2].val==FREE)){
		if(node_map[y-1][x+1].g==0){
			set_cost(node_map[y-1][x+1].x,node_map[y-1][x+1].y,x,y);
		}
		memcpy(&current_neighbours[1], &node_map[y-1][x+1], sizeof(current_neighbours[1]));//north-east
	}
	else {
		current_neighbours[1].val=1;
	}
	if (((y+1)<Y_SIZE) && ((y+1)>=0) && ((x+1)<X_SIZE) && ((x+1)>=0)&&(current_neighbours[2].val==FREE)&&(current_neighbours[4].val==FREE)){
		if(node_map[y+1][x+1].g==0){
			set_cost(node_map[y+1][x+1].x,node_map[y+1][x+1].y,x,y);
		}
		memcpy(&current_neighbours[3], &node_map[y+1][x+1], sizeof(current_neighbours[3]));//south-east
	}
	else {
		current_neighbours[3].val=1;
	}
	if (((y-1)<Y_SIZE) && ((y-1)>=0) && ((x-1)<X_SIZE) && ((x-1)>=0)&&(current_neighbours[0].val==FREE)&&(current_neighbours[6].val==FREE)){
		if(node_map[y-1][x-1].g==0){
			set_cost(node_map[y-1][x-1].x,node_map[y-1][x-1].y,x,y);
		}
		memcpy(&current_neighbours[7], &node_map[y-1][x-1], sizeof(current_neighbours[7]));//north-west
	}
	else {
		current_neighbours[7].val=1;
	}
	if (((y+1)<Y_SIZE) && ((y+1)>=0) && ((x-1)<X_SIZE) && ((x-1)>=0)&&(current_neighbours[4].val==FREE)&&(current_neighbours[6].val==FREE)){
		if(node_map[y+1][x-1].g==0){
			set_cost(node_map[y+1][x-1].x,node_map[y+1][x-1].y,x,y);
		}
		memcpy(&current_neighbours[5], &node_map[y+1][x-1], sizeof(current_neighbours[5]));//south-west
	}
	else {
		current_neighbours[5].val=1;
	}
}

//function to get the g cost a given cell with respect to its parent
int get_g_cost(int cx, int cy, int x, int y){
	if ((cx != x) && (cy != y)){
		return (node_map[cy][cx].g + DIAGONAL_COST);
	}
	else {
		return (node_map[cy][cx].g + VH_COST);
	}
}

//an array to store the path from the start to the goal after computing using a*
int path[MAP_AREA];

//function to get distance from the start node to the goal node
int get_distance(int start_x, int start_y, int goal_x, int goal_y){
	int x, y,px,py, count;
	px=goal_x;
	py=goal_y;
	count = 0;
	while (!((px==start_x)&&(py==start_y))){
		x=px;y=py;
		px = node_map[y][x].parentx;
		py = node_map[y][x].parenty;
		count++;
	}
	displayTextLine(2, "dist: %d steps", count);
	wait1Msec(100);
	return count;
}

//function to find goal from a given start point
void findGoal(int start_x, int start_y, int goal_x, int goal_y){
	int lowestx;
	int lowesty;

	init_map();

	createopen();
	insert_by_priority(node_map[start_y][start_x]);

	while(true){
		get_lowest_f_cost(lowestx, lowesty);
		add_to_closed(lowestx, lowesty);
		if(is_goal(lowestx, lowesty,goal_x,goal_y)){
			displayTextLine(2, "found path");
			wait1Msec(100);
			//get_path();
			get_distance(start_x, start_y, goal_x, goal_y);
			return;
		}
		//get neighbours
		get_neighbours(lowestx, lowesty);
		for(int i=0; i<CONNECTIVITY;i++){
			if((current_neighbours[i].val==OBST)||(current_neighbours[i].onclosed==true)){}
			else if((current_neighbours[i].g>(get_g_cost(current_neighbours[i].x,current_neighbours[i].y,lowestx, lowesty)))||
				current_neighbours[i].onopen==false){
				set_cost(current_neighbours[i].x,current_neighbours[i].y,lowestx, lowesty);
				current_neighbours[i].parentx=lowestx;
				current_neighbours[i].parenty=lowesty;
				memcpy(&node_map[current_neighbours[i].y][current_neighbours[i].x],&current_neighbours[i],sizeof(node_map[current_neighbours[i].y][current_neighbours[i].x]));
				if(current_neighbours[i].onopen==false){
					insert_by_priority(current_neighbours[i]);
				}
			}
		}
	}

}
task main()
{
	displayCenteredTextLine(0, "Station");
	displayCenteredBigTextLine(1, "RoboTaxi");
	displayCenteredTextLine(3, "Using A*");
	displayCenteredTextLine(5, "Ashesi");
	displayCenteredTextLine(6, "Fall 2015");
	sleep(5000);

	eraseDisplay();
	
	//store input from the numeric keypad
	base_station();

	//displays values received as location and destination
	displayTextLine(1, "Test Values: ");
	displayTextLine(2, "Xval: %d", passenger_location_x);
	displayTextLine(3, "Yval: %d", passenger_location_y);
	displayTextLine(4, "XDest: %d", passenger_destination_x);
	displayTextLine(5, "YDest: %d", passenger_destination_y);
	wait1Msec(100);
	goal_x = passenger_destination_x;
	goal_y = passenger_destination_y;

	//find goal with taxi1 in mind
	findGoal(taxi1base_x, taxi1base_y, passenger_location_x, passenger_location_y);
	//get distance from taxi1 to passenger
	int dist1 = get_distance(taxi1base_x, taxi1base_y, passenger_location_x, passenger_location_y);
	//find goal with taxi2 in mind
	findGoal(taxi2base_x, taxi2base_y, passenger_location_x, passenger_location_y);
	//get distance from taxi2 to passenger
	int dist2 = get_distance(taxi2base_x, taxi2base_y, passenger_location_x, passenger_location_y);

	//compare distances between taxis and passengers. select taxi with shorter distance
	if (dist1 < dist2){//if taxi1 is shorter
		eraseDisplay();
		displayTextLine(1, "taxi 1 ");
		wait1Msec(500);
		sendMessageWithParm(TAXI1LOC,passenger_location_x,passenger_location_y);//11 is location to taxi1
		wait1Msec(500);
		displayTextLine(1, "to taxi 1.1 ");
		sendMessageWithParm(TAXI1DEST,passenger_destination_x,passenger_destination_y);//12 is destination to taxi1
		wait1Msec(500);
		displayTextLine(1, "to taxi 1.2 ");
	}
	else {//if taxi2 is shorter
		displayTextLine(1, "taxi 2 ");
		wait1Msec(500);
		sendMessageWithParm(TAXI2LOC,passenger_location_x,passenger_location_y);//21 is location to taxi2

		wait1Msec(500);
		displayTextLine(1, "to taxi 2.1 ");
		sendMessageWithParm(TAXI2DEST,passenger_destination_x,passenger_destination_y);//22 is destination to taxi2
		wait1Msec(500);
		displayTextLine(1, "to taxi 2.2 ");
	}
}