Drew_MUC.c

#pragma config(Sensor, S1,     leftColor,      sensorEV3_Color)
#pragma config(Sensor, S2,     middleColor,    sensorEV3_Color)
#pragma config(Sensor, S3,     rightColor,     sensorEV3_Color)
#pragma config(Sensor, S4,     ultraSonic,     sensorEV3_Ultrasonic)
#pragma config(Motor,  motorA,          usMotor,       tmotorEV3_Medium, PIDControl, encoder)
#pragma config(Motor,  motorB,          leftMotor,     tmotorEV3_Large, PIDControl, driveLeft, encoder)
#pragma config(Motor,  motorC,          rightMotor,    tmotorEV3_Large, PIDControl, driveRight, encoder)
#pragma config(Motor,  motorD,          arm,           tmotorEV3_Large, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma DebuggerWindows("debugStream")

bool red;
bool speedBoost = false; //boolean flag for accelerating during straight aways

//USER SPECIFICATIONS
//
//  sb1 = 0
//  sb2 = 56
int home = 56;
// 0 = North
// 1 = East
// 2 = South
// 3 = West
short direction = 2;

int pathDistance = 0;
//  1 = 57
//	2 = 62
//  3 = 58
//  4 = 59
//	5 = 63
//  6 = 60
//	7 = 64
// 	8 = 65
//  9 = 61
// 10 = 66
short x = 61;              //The Node Numbers of the Three Parking lots to be visited //Order of nodes doesn't matter
short y = 60;							//3,4,6   //3,4,9   //3,6,9
short z = 63;

short package[] = {50,61}; //nodes that the package is between


float colors[][] = {
//blk,wht,red,blue
 {6, 66, 45, 10},//{7, 	71, 57, 12},	//left
 {5, 58, 41, 10},//{6, 	79, 64, 11},	//middle
 {5, 64, 45, 10},//{6, 	78, 62, 12}		//right
};

//CONSTANTS
//////Driving
#define threshold  9 // Brightness threshold for when the line following funtion will end. This needs to be a number between the readings for Black and Blue
#define leftAvg (colors[0][1] - colors[0][0]) / 2 //Brightness average between Black and White. Used for proportional edge following
#define middleAvg (colors[1][1] - colors[1][0]) / 2 //Brightness average between Black and White. Used for proportional edge following
#define rightAvg (colors[2][1] - colors[2][0]) / 2 //Brightness average between Black and White. Used for proportional edge following
#define sensitivity  0.3 //Determines how sensitive the turning is for edge following
#define RoWsensitivity 30 //Determines how far away an object has to be to not be detected by the ultrasonic sensor
#define usTurn 75 //How many degrees the ultrasonic sensor wil turn when panning
#define slow 15 //The speed at with the robot is moving in a controlled manner. 20 seems to be as slow as the RVW will allow a robot to move
#define leftSensor 0 //index of the colors array for the left color sensor
#define middleSensor 1 //index of the colors array for the middle color sensor
#define rightSensor 2 //index of the colors array for the right color sensor
#define blackC 0 //index of the colors array for black
#define whiteC 1 //index of the colors array for white
#define redC 2 //index of the colors array for red
#define blueC 3 //index of the colors array for blue

//////Pathfinding
#define MAX_LIST_SIZE 120 //Maximum possible size of any linked list

//VARIABLES
short currentNode = home; //Signifies the node the robot is currently on, doesn't update until the robot gets to the red line of the next node
int speed = 30; //The speed at which the robot will drive when performing the line following function between nodes
//int directions[] = {0,1,1,1,1,0,0,1};    //{0,0,1,0,0,1,2,1}{2,0,0,1,1,0,0,1};
int dirCtr = 0; //Keeps track of how many turns the robot has already encountered
int parking[] = {2,2,2}; //Tells the robot to park in particular parking spots
int parkCtr = 0; //Keeps track of how many parking lots the robot has already encountered
short directions[100];// Array of turning directions between two nodes.
short finalNodeList[100]; //Array of every node the robot travels to from start to finish
int path = 0; //The index of the tuple array that corresponds to the correct order of parking lots

//STRUCTURES
struct tListNode
{
	tListNode *next;
	// neighbouring node

	short value;
	// value held by node
} tListNode;

struct tList
{
	tListNode *head;
	// pointer to the node at the head of the list

	tListNode *tail;
	// pointer to the node at the top of the list

	int size;
	// current size of the list
} tList;


short int tuple[][] = { // This is the array that provides every possible combination of paths to help determine which is the shortest
{home,x,y,z,home,0},
{home,x,z,y,home,0},
{home,y,x,z,home,0},
{home,y,z,x,home,0},
{home,z,x,y,home,0},
{home,z,y,x,home,0}
};


//MAP DATA
short int nodeData[][] = { //Index of array = node num.
// x    y   c1  c2   $  parent
	{5,		13,	1,	1,	0,	-1},	//0 Home Base 1
	{1,		13,	2,	2,	0,	-1},	//1
	{1,		1,	57,	57,	0,	-1},	//2
	{8,		1,	4,	4,	0,	-1},	//3
	{13,	1,	23,	23,	0,	-1},	//4
	{10,	3,	4,	4,	0,	-1},	//5
	{10,	5,	58,	5,	0,	-1},	//6
	{10,	10,	6,	10,	0,	-1},	//7
	{10,	13,	0,	7,	0,	-1},	//8
	{11,	12,	7,	7,	0,	-1},	//9
	{14,	10,	62,	62,	0,	-1},	//10
	{17,	7,	12,	12,	0,	-1},	//11
	{17,	4,	58,	58,	0,	-1},	//12
	{0,		0,	0,	0,  0,	-1},	//13
	{22,	1,	23,	23,	0,	-1},	//14
	{22,	4,	14,	22,	0,	-1},	//15
	{22,	7,	11,	15,	0,	-1},	//16
	{22,	11,	59,	20,	0,	-1},	//17
	{22,	13,	8,	17,	0,	-1},	//18
	{27,	13,	18,	18,	0,	-1},	//19
	{27,	11,	19,	30,	0,	-1},	//20
	{27,	7,	16,	20,	0,	-1},	//21
	{27,	4,	21,	25,	0,	-1},	//22
	{27,	1,	22,	55,	0,	-1},	//23
	{32,	1,	55,	55,	0,	-1},	//24
	{32,	4,	36,	24,	0,	-1},	//25
	{32,	5,	25,	25,	0,	-1},	//26 Roundabout
	{30,	7,	21,	26,	0,	-1},	//27 Roundabout
	{34,	7,	27,	27,	0,	-1},	//28 Roundabout
	{32,	9,	27,	27,	0,	-1},	//29 Roundabout
	{32,	11,	53,	29,	0,	-1},	//30 /////////Changed for roundabout integration
	{32,	13,	18,	30,	0,	-1},	//31
	{35,	13,	31,	31,	0,	-1},	//32
	{38,	13,	31,	34,	0,	-1},	//33
	{38,	11,	42,	63,	0,	-1},	//34
	{38,	7,	36,	28,	0,	-1},	//35 /////////Changed for roundabout integration
	{38,	4,	60,	37,	0,	-1},	//36
	{38,	1,	38,	38,	0,	-1},	//37
	{42,	1,	39,	45,	0,	-1},	//38
	{42,	4,	40,	43,	0,	-1},	//39
	{42,	7,	35,	35,	0,	-1},	//40
	{45,	13,	42,	33,	0,	-1},	//41
	{45,	11,	65,	64,	0,	-1},	//42
	{45,	4,	44,	46,	0,	-1},	//43
	{45,	1,	45,	45,	0,	-1},	//44
	{50,	1,	46,	56,	0,	-1},	//45
	{50,	4,	47,	50,	0,	-1},	//46
	{50,	11,	48,	48,	0,	-1},	//47
	{50,	13,	41,	41,	0,	-1},	//48
	{54,	13,	41,	41,	0,	-1},	//49
	{54,	4,	61,	66,	0,	-1},	//50
	{54,	1,	50,	50,	0,	-1},	//51
	{45,	7,	40,	43,	0,	-1},	//52
	{35,	11,	32,	34,	0,	-1},	//53
	{35,	4,	36,	36,	0,	-1},	//54
	{35,	1,	54,	38,	0,	-1},	//55
	{51,  1,  51, 51, 0,	-1},  //56 Home Base 2
	{3,		1,	3,	3,	0,	-1},  //57 *Parking Lot 1
	{18,	4,	15,	15,	0,	-1},	//58 *Parking Lot 3
	{22,	10,	16,	16,	0,	-1},	//59 *Parking Lot 4
	{39,	4,	39,	39,	0,	-1},	//60 *Parking Lot 6
	{54,	7,	49,	49,	0,	-1},	//61 *Parking Lot 9
	{18,	11,	17,	17,	0,	-1},	//62 *Parking Lot 2
	{38,	10,	35,	35,	0,	-1},	//63 *Parking Lot 5
	{45,	10,	52,	52,	0,	-1},	//64 *Parking Lot 7
	{46,	11,	47,	47,	0,	-1},	//65 *Parking Lot 8
	{54,	7,	49,	49,	0,	-1},	//66 *Parking Lot 10
};



tList myList; //myList is a linked list of the nodes on the path dijkstra has calculated
tList finalList; // finalList is the linked list of turning directions that calcDirections has produced
tListNode _listNodePool[MAX_LIST_SIZE]; 	// array of nodes in a pool
bool _allocStatus[MAX_LIST_SIZE]; // parallel array of booleans to signify if a node is occupied

//METHODS
//////DRIVING

void drive(int l, int r) //Sets motor speeds to specified power levels
{
	setMotorSpeed(leftMotor, l);
	setMotorSpeed(rightMotor, r);
}

void halt()		//stops both drive motors and waits 1/10 of a second
{
	setMotorSpeed(leftMotor, 0);
	setMotorSpeed(rightMotor, 0);
}

void resetEncoders()		//sets drive motor encoder values to 0
{
	nMotorEncoder(leftMotor) = 0;
	nMotorEncoder(rightMotor) = 0;
}

void drive(int l, int r, float dist) //runs both drive motors at specified power levels until either one reaches limit(degrees)
{
	resetEncoders();
																// These lines of code are specific to RVW to set the minimum motor speed so that the motors move
	/*if(l<16 && l> 0) l = 16;
	else if(l>16 && l<0) l = -16;
	if(r<16 && r> 0) r = 16;
	else if(r>16 && r<0) r = -16;*/

	setMotorSpeed(leftMotor, l);
	setMotorSpeed(rightMotor, r);

//	if(dist >= 0) //This stops the robot from moving when either wheel has turned the desireable amount
		while(abs(getMotorEncoder(leftMotor)) < abs(dist) && abs(getMotorEncoder(rightMotor)) < abs(dist));

	halt();
}

void lf_right(int dist) //performs edge following with the right-most color sensor until middle sensor reading exceeds threshold
{
	resetEncoders();
	while (nMotorEncoder(rightMotor) < dist)
	{
		float turn;
		turn = (getColorReflected(rightColor) - rightAvg) * sensitivity;
		drive(speed - turn, speed + turn);
	}
}

void lf_right() //performs edge following with the right-most color sensor until left sensor reading is not red
{
	while(getColorName(leftColor) != 5)
	{
		float turn;
		turn = (getColorReflected(rightColor) - rightAvg) * sensitivity;
		drive(speed - turn, speed + turn);
	}
}

void lf_left()		//performs edge following with the left-most sensor until the middle color sensor detects a line
{
	//writeDebugStreamLine("%i", getColorReflected(middleColor));

	while (getColorReflected(middleColor) < threshold && getUSDistance(ultraSonic) > 10)
	{
		float turn;
		turn = (getColorReflected(leftColor) - leftAvg) * sensitivity;
		drive(speed + turn, speed - turn);
	}
	halt();
}

void lf_left(int dist) //performs edge following with the left-most sensor for a given degrees
{
	resetEncoders();
	while (abs(getMotorEncoder(leftMotor)) < abs(dist))
	{
		float turn;
		turn = (getColorReflected(leftColor) - leftAvg) * .5;
		drive(80 + turn, 80 - turn);
	}
}

void medMotor(int speed, int dist)		//runs the medium motor at the specified power for the specified distance.
{
	nMotorEncoder(usMotor) = 0;
	setMotorSpeed(usMotor, speed);

	while(abs(nMotorEncoder(usMotor)) < abs(dist) );

	setMotorSpeed(usMotor, 0);
	wait1Msec(100);
}

void rOfWay(short dir) //determines and perfoms right of way
{
	switch (dir)
	{
		case 0:		//turn left
			medMotor(25, usTurn);
			wait1Msec(500);

			if (getUSDistance(ultraSonic) < RoWsensitivity)
			{
				drive(-slow, -slow, -0.4*360);
				medMotor(-25, -usTurn);
				wait1Msec(9000);
				do {wait1Msec(3000);}
				while (getUSDistance(ultraSonic) < RoWsensitivity);
				wait1Msec(2000);
				drive(slow, slow, 0.4*360);
			}
			else
				medMotor(-25, -usTurn);

			break;

		case 1:		//go straight
			medMotor(-25, -usTurn);
			wait1Msec(500);

			if (getUSDistance(ultraSonic) < RoWsensitivity)
			{
				drive(-slow, -slow, -0.4*360);
				medMotor(25, usTurn);
				wait1Msec (3000);
				do {wait1Msec(3000);}
				while (getUSDistance(ultraSonic) < RoWsensitivity);
				wait1Msec(2000);
				drive(slow, slow, 0.4*360);
			}
			else
			{
				medMotor(25, usTurn*2);			//70+70 = 140
				wait1Msec (500);
				if (getUSDistance(ultraSonic) < RoWsensitivity)
				{
					drive(-slow, -slow, -0.4*360);
					medMotor(-25, -usTurn);
					wait1Msec(9000);
					do {wait1Msec(3000);}
					while (getUSDistance(ultraSonic) < RoWsensitivity);
					wait1Msec(2000);
					drive(slow, slow, 0.4*360);
				}
				else
					medMotor(-25, -usTurn);
			}
			break;

		case 2:		//turn right
			medMotor(-25, -usTurn);
			wait1Msec(500);

			if (getUSDistance(ultraSonic) < RoWsensitivity)
			{
				drive(-slow, -slow, -0.4*360);
				medMotor(25, usTurn);
				wait1Msec(3000);
				do {wait1Msec(3000);}
				while (getUSDistance(ultraSonic) < RoWsensitivity);
				wait1Msec(2000);
				drive(slow, slow, 0.4*360);
			}
			else
				medMotor(25, usTurn);
			break;
	}
}

void turn(short dir) //performs a turn based on given integer
{
	//rOfWay(dir);
	//wait1Msec(1000);
	switch (dir)
	{
		case 0:		//turn left
			drive(slow, slow, 357);			//drive forward
			resetEncoders();
			drive(-20, 20);
			while (getColorReflected(leftColor) < threshold);
			halt();
			direction = ((direction+3)%4);
			break;

		case 1:		//go straight
			drive(slow,slow,360*1.7);
			drive(slow,-slow,40);
			resetEncoders();
			drive(-slow,slow);
			while (getColorReflected(leftColor) < threshold);
			halt();
			break;

		case 2:		//turn right
			drive(slow, slow, 357);			//drive forward
			resetEncoders();
			drive(slow, -slow);
			while (getColorReflected(rightColor) < threshold);
			lf_right(180);
			halt();
			direction = ((direction+1)%4);
			break;
	}

}

void parkLeft(int spot, bool home) //performs parking based on the spot given as an integer.
{
	if(spot != 0)
	{
		spot--;
		for(int i=0; i<=spot; i++)
		{
			drive(slow,slow,0.2*360);
			while(getColorReflected(leftColor) < threshold + 10)
			{
				float turn;
				turn = (getColorReflected(middleColor) - (((colors[middleSensor][blackC] + colors[middleSensor][blueC]) / 2) - 1)) * 5;
				drive(slow + turn, slow - turn);
			}
		}

		medMotor(25, usTurn);

		//drive(slow,slow,0.2*360);
		drive(0,-20,-1.0*360);
		drive(-slow,-slow,-0.7*360);
		wait1Msec(3100);

		if(home)
			stopAllTasks();

		drive(slow,slow,1.15*360);
		drive(-20,20,180);
		drive(-20,-20,-120);
		/*drive(-20,20);
		while(getColorReflected(leftColor) < threshold);*/
		halt();
	}
	else
	{
		drive(-slow,-slow,-180);
		drive(slow,-slow,40);
	}
}

void park66(int spot, bool home) //performs parking based on the spot given as an integer.
{
	if(spot != 0)
	{
		spot--;
		for (int i=0; i<=spot; i++)
		{
			drive(slow,slow,0.2*360);
			while (getColorReflected(rightColor) < colors[rightSensor][blueC] + 2.5)
			{
				float turn;
				turn = (getColorReflected(leftColor) - ((colors[leftSensor][blackC] + colors[leftSensor][blueC]) / 2)) * 4;
				drive(speed + turn, speed - turn);
			}
		}
		drive(slow,slow,0.2*360);
		drive(-20, 0, -1.0*360);
		drive(-slow,-slow,-.8*360);
		wait1Msec(3100);

		if(home)
			stopAllTasks();

		drive(slow,slow,1.15*360);
		drive(20, -20,180);
		drive(-20,-20,-120);
		/*drive(10, -10);
		while(getColorReflected(rightColor) < ((colors[rightSensor][blackC] + colors[rightSensor][blueC]) / 2));*/
		halt();
	}
	else
	{
		drive(-slow, -slow,-180);
		drive(-slow, slow,40);
	}
}

void parkRight(int spot, bool home) //performs parking based on the spot given as an integer.
{
	if(spot != 0)
	{
		spot--;
		for (int i=0; i<=spot; i++)
		{
			drive(slow,slow,0.2*360);
			while (getColorReflected(rightColor) < colors[rightSensor][blueC] + 2)
			{
				float turn;
				turn = (getColorReflected(leftColor) - (leftAvg - 5)) * sensitivity;
				drive(speed + turn, speed - turn);
			}
		}
		drive(slow,slow,0.2*360);
		drive(-20, 0, -1.0*360);
		drive(-slow,-slow,-.8*360);
		wait1Msec(3100);

		if(home)
			stopAllTasks();

		drive(slow,slow,1.15*360);
		drive(20, -20,180);
		drive(-20,-20,-120);
		/*drive(10, -10);
		while(getColorReflected(rightColor) < ((colors[rightSensor][blackC] + colors[rightSensor][blueC]) / 2));*/
		halt();
	}
	else
	{
		drive(-slow, -slow,-180);
		drive(-slow, slow,40);
	}
}

bool isRed()		//checks if the middle sensor is reading red
{
	return (getColorReflected(middleColor) > ((colors[middleSensor][redC] + colors[middleSensor][blueC]) / 2)) ? true : false;
}


//////PATHFINDING
void initList(tList *list)
{
	// Reset all the value to 0
	memset(_listNodePool, 0, sizeof(tListNode) * MAX_LIST_SIZE);
	memset(_allocStatus, false, sizeof(bool) * MAX_LIST_SIZE);

	// Set the current head and tail to the first node
	list->head = NULL;
	list->tail = NULL;

	// Set the size to 0
	list->size = 0;
}

void insertNode(tList *list, tListNode *newNode)
{
	if(list->size == 0)// Is the list empty?
	{
		list->head = newNode;
		list->tail = newNode;
		list->size++;
	}
	// node == NULL, so put the newNode at the start
	// of the list
	else
	{
		newNode->next = list->head;
		list->head = newNode;
		list->size++;
		// if the node was the tail, update this pointer to the newNode
	}
}

tListNode * NEW() //returns pointer to the first free node
{
	for(int i = 0; i < MAX_LIST_SIZE; i++)
	{
		// Return the node if it is not already allocated
		if(!_allocStatus[i])
		{
			_allocStatus[i] = true;
			return _listNodePool[i];
		}
	}
	// No free nodes available
	return NULL;
}

short calcDist (short x1, short y1, short x2, short y2) //Method used to calculate the distance between any two sets of coordinates
{
	if(x2 == 34 && y2 == 7)
			return 6;
	else if (x2 == 36 && y2 == 9)
			return 3;
	else
		return sqrt(pow((x1-x2), 2) + pow((y1-y2), 2));
}

short traverse(tListNode * ptr, short index) //method used to look for an element of a linked list at a given index
{
	tListNode * r;
	r = ptr;
	for(int i = 0; ((i < index) && (r->next != NULL)); i++)
	{
		r = r->next;
	}
	return r->value;
}

bool contains(tListNode * ptr, short element) //method used to check if a specified value is fould in the linked list
{
	tListNode * r;
	r = ptr;
	while (r != NULL)
	{
		if (r->value == element)
			return true;
		r = r->next;
	}
	return false;
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

short dijkstra(short a, short b)//Oh Dear. I'm sorry you're here reading this. I wish you good luck...

{
	initList(myList); //Initializes the list of solved nodes
	initList(finalList); //Initializes the list of nodes that are used in the path

	tListNode * nodePtr; //Declares node pointer called nodePtr

	nodePtr = NEW(); //Sets nodePtr equal to an open node
	nodePtr->value = a;
	insertNode(myList, nodePtr); //inserts the node into the list


	while(!contains(myList.head, b))//main loop, stop at target
	{
		short shortNode = -1;

		for(int i = 0; i < myList.size; i++)//iterates through solved nodes
		{
			short parent = traverse(myList.head, i);
			for(int j = 2; j < 4; j++) //iterates through children
			{
				if(!contains(myList.head, nodeData[parent][j])) //checks if the child being viewed is not already in the solved list
				{
					shortNode = nodeData[parent][j]; // sets shortNode to the first unsolved node adjacent that is a child to a solved node
					short cCost = (calcDist(nodeData[parent][0], nodeData[parent][1], nodeData[nodeData[parent][j]][0], nodeData[nodeData[parent][j]][1]) + nodeData[parent][4]);
					if( cCost < nodeData[shortNode][4] || nodeData[shortNode][4] == 0)
					{
						nodeData[shortNode][4] = cCost;
						nodeData[shortNode][5] = parent;
					}
					j = 4;
					i = myList.size;
				}
			}
		}

		for(int i = 0; i < myList.size; i++) //iterating through solved Nodes
		{
			short parent = traverse(myList.head,i); // index of the current solved node
			for(int j = 2; j < 4; j++) //iterating through the children of the solved nodes
			{
				if (!contains(myList.head,nodeData[parent][j])) // checks if the children of the solved node haven't been solved
				{
					short cCost = (calcDist(nodeData[parent][0], nodeData[parent][1], nodeData[nodeData[parent][j]][0], nodeData[nodeData[parent][j]][1]) + nodeData[parent][4]);
					//Alright, you are setting the cost of the first/second child(depending on the value of j) of currNode to the distance between currNode and it's child plus the cost of the parent

					if (nodeData[nodeData[parent][j]][4] == 0) //does the current child not have a cost value?
					{
						nodeData[nodeData[parent][j]][4] = cCost; // If not, then the child's cost is written
						// delete nodeData[shortNode][5] = parent; // The parent of the new shortnode is updated
					}
					else if(cCost < nodeData[nodeData[parent][j]][4]) //If so, is it's new cost lower?
					{
						nodeData[nodeData[parent][j]][4] = cCost; // If so, then the new, lower cost is written
						// delete nodeData[shortNode][5] = parent; // The parent of the new shortnode is updated
					}
					if(nodeData[nodeData[parent][j]][4] < nodeData[shortNode][4]) //Checks if the new node has a lower cost than the standing shortnode
					{																		//If so,
						shortNode = nodeData[parent][j]; // The value of shortnode is updated
						nodeData[shortNode][4] = cCost;
						nodeData[shortNode][5] = parent; // The parent of the new shortnode is updated
					}
				}
			}
		}

		nodePtr = NEW();
		nodePtr->value = shortNode;
		insertNode(myList, nodePtr); //This adds the shortest node to the linked list of solved nodes
	}
	/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	nodePtr = NEW();
	nodePtr->value = b;
	insertNode(finalList, nodePtr);
	while(!contains(finalList.head, a))
	{
		nodePtr = NEW();
		short temp = finalList.head -> value;
		nodePtr->value = nodeData[temp][5];
		insertNode(finalList, nodePtr);
	}
	short TotalCost = nodeData[b][4]; // the total cost(in arbitrary grid units) to drive the path

	for(int i = 0; i<62; i++) //Resets the nodeData array columns cost and parent so that Dijkstra can be run again
	{
		nodeData[i][4] = 0;
		nodeData[i][5] = -1;
	}

	return TotalCost;
}

short AStar(short a, short b) //dynamic pathfinding algorithm: heuristic needs to be 0 for optimal path (so Dijkstra's)
{
	tList solved;
	initList(solved); //Initializes the list of solved nodes
	initList(finalList); //Initializes the list of nodes that are used in the path

	tListNode * homeNode = NEW(); //starting node
	homeNode->value = a;
	insertNode(solved, homeNode);

	while(!contains(solved.head, b))
	{
		short minCost = 1000000;
		short cheapest = 0;

		for (int i = 0; i < solved.size; i++)
		{
			for (int j = 2; j < 4; j++)
			{
				short child = nodeData[traverse(solved.head, i)][j];

				if (!contains(solved.head, child))
				{
					short cost = calcDist(nodeData[traverse(solved.head, i)][1], nodeData[traverse(solved.head, i)][2],
						nodeData[child][1], nodeData[child][2]) + nodeData[traverse(solved.head, i)][4];// +
						//abs(nodeData[child][1] - nodeData[b][1]) + abs(nodeData[child][2] - nodeData[b][2]);
						//calculates the cost of the current node

					if (nodeData[child][4] == 0 || cost < nodeData[child][4]) //if the calculated cost is less than the current cost it updates the cost and the parent
					{
						nodeData[child][4] = cost;
						nodeData[child][5] = traverse(solved.head, i);
					}
					if (cost < minCost) //if the calculated cost is less than the current minimum cost, it updates the minimum cost and cheapest child
					{
						minCost = cost;
						cheapest = child;
					}
				}
			}
		}

		tListNode * cheap = NEW();
		cheap->value = cheapest;
		insertNode(solved, cheap); //adds the cheapest child to the list

		minCost = 1000000; //resets minCost and cheapest
		cheapest = 0;
	}

	tListNode * curr = NEW();
	curr->value = b;

	while (curr->value != - 1) //starts at the end and looks at all of the parents to make a direct path from start to finish
	{
		insertNode(finalList, curr);

		tListNode * temp = NEW();
		temp->value = nodeData[curr->value][5];
		curr = temp;
	}

	short TotalCost = nodeData[b][4]; // the total cost(in arbitrary grid units) to drive the path

	for(int i = 0; i < 67; i++) //Resets the nodeData array columns cost and parent so that Dijkstra can be run again
	{
		nodeData[i][4] = 0;
		nodeData[i][5] = -1;
	}

	return TotalCost; // returns the total cost(in arbitrary grid units) to drive the path
}

short calcTurn(short curNode, short nxtNode) //calculates the direction nxtNode is relative to curNode
{
	if(abs(nodeData[curNode][0]-nodeData[nxtNode][0]) > abs(nodeData[curNode][1]-nodeData[nxtNode][1]))
		if(nodeData[curNode][0]-nodeData[nxtNode][0] > 0)
			return 3;
		else
			return 1;
	else
		if(nodeData[curNode][1]-nodeData[nxtNode][1] > 0)
			return 2;
		else
			return 0;
}

void calcDirections()// Reads from myList and writes to finalList. Calculates which way the robot must turn at each intersection
{
	short temp = direction;// saves direction to a temporary variable because it changed as the program runs through the turns. Direction is later set equal to the temp variable
	short target;
	int i;// This iterator variable is initialized before the start of the loop so that it doesn't go out of scope and its value is saved after the loop is completed
	///if(strt == 0 || strt == 56) dirCtr = 1;
	//else
	dirCtr = 0; //resets the direction counter so that the drive code has a fresh start
	for(i = 0; i < finalList.size-1; i++)//cycles through every node
	{
		short curNode, nxtNode;
		curNode = traverse(finalList.head, i);
		nxtNode = traverse(finalList.head, i+1);

		target = calcTurn(curNode, nxtNode);

		directions[i] = (abs((direction - target) - 1) % 4);
		direction = target;

		writeDebugStreamLine("%d", directions[i]);
	}
	if(traverse(finalList.head, i + 1) == home)
		directions[i] = -1;
	else if(traverse(finalList.head, i + 1) > 56)
		directions[i] = -2;
	else //if(traverse(finalList.head, i + 1) == package[0])
		directions[i] = -3;

	writeDebugStreamLine("%d", directions[i]);

	direction = temp;//direction
}

bool drive()//Handles all of the driving. Calls turning and parking methods when applicable. Returns when the linked list reads "-2"
{
	while(directions[dirCtr] != -2 && directions[dirCtr] != -1 && directions[dirCtr] != -3)
	{
		if (dirCtr != 1 && dirCtr != 0 && speedBoost) //line follows for the distance between any two nodes that aren't in the beginning of the list
			lf_left((calcDist(nodeData[traverse(finalList.head, dirCtr - 1)][0], nodeData[traverse(finalList.head, dirCtr - 1)][1], nodeData[traverse(finalList.head, dirCtr)][0], nodeData[traverse(finalList.head, dirCtr)][1]) * 350) - 450);

		writeDebugStreamLine("%d", directions[dirCtr]);

		lf_left(); //line follows until the middle sensor passes the threshold
		drive(slow,slow,7); //drives forward slightly
		wait1Msec(500);
		red = isRed(); //checks if the current color is red

		if (red) //if it is red
		{
			short brightness = getColorReflected(middleColor); //checks color again(to differentiate between yellow and red)
			wait1Msec(100);
			brightness = getColorReflected(middleColor);

			if(brightness < 70) // if it is red
			{
				currentNode = traverse(finalList.head, dirCtr);
				turn(directions[dirCtr++]);//turn as normal
			}
			else
			{
				writeDebugStreamLine("Blockade"); // roadblock
				drive(-10, -10, -20);
				return true;
			}
		}
		else
		{
			drive(20, -20, 70); //passes the parking lot to the left
			while (getColorReflected(leftColor) < colors[leftSensor][blueC] + 5)//((colors[leftSensor][whiteC] + colors[leftSensor][blueC]) / 2))
			{
				float turn;
				turn = (getColorReflected(leftColor) - ((colors[leftSensor][blackC] + colors[leftSensor][blueC]) / 2)) * 4;
				drive(speed + turn, speed - turn);
			}
		}

		if (traverse(finalList.head, dirCtr + 1) > 56 && directions[dirCtr] != -2) //if the next node is a parking lot but not the final node
			dirCtr ++;
	}

	writeDebugStreamLine("Exiting Loop");

	if (directions[dirCtr] == -1) //If you are back home
	{
		//Line follows with the right sensor until the left sensor detects red
		drive(-10, 10, 10);
		lf_right();
		//Turns to backup
		drive(slow, slow, 360);
		drive(slow, - slow, 180);
		drive(-slow, -slow);
		while(getColorReflected(middleColor) < ((colors[middleSensor][blackC] + colors[middleSensor][redC]) / 2)); //backs up until middle sensor detects red
		drive(-slow, -slow, -50);
		halt();
	}
	else if (directions[dirCtr] == -2) //parking
	{
		if (finalList.tail->value < 62)
		{
			lf_left();
			while(getColorName(leftColor) != 2)
				drive(20,-20);
			parkLeft(parking[parkCtr++], false);
		}
		else
		{
			while (getColorName(rightColor) == 6)//getColorReflected(rightColor) > ((colors[rightSensor][whiteC] + colors[rightSensor][blueC]) / 2))
			{
				float turn;
				turn = (getColorReflected(leftColor) - leftAvg) * sensitivity;
				drive(speed + turn, speed - turn);
			}
			drive(20, 15, 50);
			halt();

			if (finalList.tail->value == 66)
				park66(parking[parkCtr++], false);
			else
				parkRight(parking[parkCtr++], false);
		}

		while (getColorReflected(leftColor) < ((colors[leftSensor][whiteC] + colors[leftSensor][blueC]) / 2))
		{
			float turn;
			turn = (getColorReflected(leftColor) - ((colors[leftSensor][blackC] + colors[leftSensor][blueC]) / 2)) * 5;
			drive(speed + turn, speed - turn);
		}

		halt();
	}
	else if (directions[dirCtr] == -3) //package pickup
	{
		lf_left();

		turn((abs((direction - calcTurn(package[0],package[1])) - 1) % 4));

		while(getColorName(rightColor) != 4)
		{
		 	static float turn;
		 	turn = (40 - getColorReflected(leftColor)) * .4;
			drive(speed - turn, speed + turn);
		}

		drive(20, 20, 370);

		resetMotorEncoder(arm);
		setMotorSpeed(arm, 50);
		wait1Msec(1000);
		setMotorSpeed(arm, 0);

		wait1Msec(1000);

		resetMotorEncoder(arm);
		setMotorSpeed(arm, -100);
		wait1Msec(500);
		setMotorSpeed(arm, 0);
	}

	return false;
}

short pathSequence()// Cycles through every possible combination of parking lot combinations and finds the shortest. Returns the corresponding index in the tuple array.
{
	short shortest = 0; //index of the shortest distance stored in column 3

	for(int i=0; i<6; i++)//Cycles through the indexes(or rows) of the tuple array
		for(int j=0; j<4; j++)// cycles through the columns of those rows
			tuple[i][5] += AStar(tuple[i][j], tuple[i][j+1]);//runs AStar and writes the final cost value to the final column in that index of the tuple array

	for(int i=1; i<6; i++)//cycles through the indexes again. This time to find the shortest arrangement
		if(tuple[i][5] < tuple[shortest][5]) //checks if current index is shorter than the shortest index
			shortest = i;// if so, then shortest is updated

	return shortest;//returns the index of the shortest arrangement of parking lots
}

void convert2Node()
{
	short convert[10] = {57,62,58,59,63,60,64,65,61,66};
	x = convert[x - 1];
	y = convert[y - 1];
	z = convert[z - 1];
}

task main() //Main loop
{
	clearDebugStream();

	AStar(56, package[0]);
	calcDirections();
	drive();

	AStar(package[1],56);
	calcDirections();
	drive();

	/*for (int i = 0; i < finalList.size; i++)
		writeDebugStreamLine("%i", traverse(finalList.head, i));

	while (true);*/

	resetMotorEncoder(arm);
	setMotorSpeed(arm, 20);
	wait1Msec(1000);
	setMotorSpeed(arm, 0);


	/*path = pathSequence();
	short index = 0;
	for(int i = 0; i<4;i++)
	{
		pathDistance += AStar(tuple[path][i],tuple[path][i+1]);
		for(int i = 0; i < finalList.size; i++)
		{
			finalNodeList[index++] = traverse(finalList.head, i);
		}
	}
	for(int i = 0; i<4;i++)
	{
		AStar(tuple[path][i],tuple[path][i+1]); //calls the AStar function between two of the parking lots
		calcDirections(); //takes the finalList and converts it into an array of turning directions

		for (int j = 0; j < finalList.size; j++)
			writeDebugStreamLine("%d -> %d: %d", tuple[path][i], tuple[path][i+1], traverse(finalList.head, j));

		if(drive()) //if drive returns true, then the robot has come upon a roadblock
		{
			short curr = traverse(myList.head, dirCtr);
			short next = nodeData[curr][2];
			if(curr == next) next = nodeData[curr][3];

			short target;
			if(abs(nodeData[curr][0]-nodeData[next][0]) > abs(nodeData[curr][1]-nodeData[next][1]))
				if(nodeData[curr][0]-nodeData[next][0] > 0)
					target = 3;
				else
					target = 1;
			else

			if(nodeData[curr][1]-nodeData[next][1] > 0)
					target = 2;
				else
					target = 0;

			short tempDir = (abs((direction - target)-1) % 4);

			if(tempDir == 0)
				drive(-slow, slow, 100);
			else
				drive(slow, -slow, 100);

			direction = target;

			drive(slow, slow, 20);
			lf_left();

			AStar(next, tuple[path][i+1]);
			calcDirections();
			drive();
		}
	}			*/
}