mazesolver.c

#pragma config(Sensor, S1,     leftTouch,      sensorEV3_Touch)
#pragma config(Sensor, S2,     rightTouch,     sensorEV3_Touch)
#pragma config(Sensor, S3,     myGyro,         sensorEV3_Gyro)
#pragma config(Motor,  motorA,          leftMotor,     tmotorEV3_Large, PIDControl, reversed, encoder)
#pragma config(Motor,  motorB,          rightMotor,    tmotorEV3_Large, PIDControl, reversed, encoder)
#pragma config(Motor,  motorC,          armMotor,      tmotorEV3_Large, PIDControl, reversed, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

// ********************************
// Last Modified by: Kourosh azizi
// Time: Nov 24
//*********************************
// Recent things that were worked on:
// 1. Moving forward function
// 2. Touch sensors
// 3. Lifting
// Testing facing the north through out the code ...

// ------- Current Physical Robot Position ------- //
int RobotCurrentRow = 0;
int RobotCurrentCol = 0;
// ----------------------------------------------- //
// ------------- Screen Size -------------- //
const int ScreenHeight = 127;
const int ScreenWidth = 177;
// ---------------------------------------- //

// ------------- Wall Cells ---------------- //
typedef struct{
	int NorthWall;
	int EastWall;
	int SouthWall;
	int WestWall;
}Cell;

Cell Grid[4][6];
// ------------------------------------------ //

// --------------- Function Prototypes --------------- //
void GridInit();
void WallGen();
void GridDraw();
void DrawBot();
void DisplayStartandEnd(int TargetPosRow, int TargetPosCol);
void checkCurrentPos();
void checkLeft();
void checkRight();
int Solver();
int CheckWall();
int *startToEnd_1(int StartPosRow, int StartPosCol, int TargetPosRow, int TargetPosCol);

// -------- moving the physical robots --------- //
void turnLeft();
void turnRight();
void robotCurrentPos();
void moveForward();
void movingToPath(int* shortestPath);
// -------------------------------------------------- //
bool checkFrontWall();
void reset();
void stackBlock();
void takeBlock();
void faceNorth();
// --------------- Global Variables --------------- //
int position_array[50]; //absolute position of every step the robot takes, will be reduced in next program
int index_of_direction = 0; //counter for position_array
int newArray[50];


int CurrentPosRow = 0;
int CurrentPosCol = 0;

// Start Facing North
int RobotDirection = 1; // 1 = North, 2 = East, 3 = South, 4 = West
// ------------------------------------------------ //

task main(){
	// step 1:
	int StartPosRow=0;	// Initial position
	int StartPosCol=0;
	int TargetPosRow1=0; // Initial Ending position
	int TargetPosCol1=1;

	//// step 2:
	int TargetPosRow2=0; // Second Ending position
	int TargetPosCol2=2;

	//// step3:
	int TargetPosRow3=0; // Third Ending position
	int TargetPosCol3=0;

	//// step4:
	//int TargetPosRow4=1; // Fourth Ending position
	//int TargetPosCol4=5;


	int* shortestPath1 = startToEnd_1(StartPosRow, StartPosCol, TargetPosRow1, TargetPosCol1);
	//movingToPath(shortestPath1);
	//takeBlock();
	faceNorth();
	int* shortestPath2 = startToEnd_1(TargetPosRow1, TargetPosCol1, TargetPosRow2, TargetPosCol2);
	//movingToPath(shortestPath2);
	faceNorth();
	//stackBlock();
	//int* shortestPath3 = startToEnd_1( TargetPosRow2, TargetPosCol2, TargetPosRow3, TargetPosCol3);
	//faceNorth();
	//int* shortestPath4 = startToEnd_1( TargetPosRow3, TargetPosCol3, TargetPosRow4, TargetPosCol4);
	//int* shortestReturn = startToEnd_1( TargetPosRow4, TargetPosCol4, StartPosRow, StartPosCol);		// coming home ...
}

int* startToEnd_1(int StartPosRow, int StartPosCol, int TargetPosRow, int TargetPosCol){
	GridInit();
	WallGen();
	//int newArray[50];
	CurrentPosRow = StartPosRow;
	CurrentPosCol = StartPosCol;

	int counter = 0;
	while((CurrentPosRow!=TargetPosRow) || (CurrentPosCol!=TargetPosCol)){
		int position_returned= Solver();
		/*
		The robot_direction is recorded everytime there's a checkRight or checkLeft.
		To fit our next program, the position_array is recorded by N=1,E=2, S=3, W=4.
		*/
		position_array[index_of_direction] = position_returned+1;
		index_of_direction++;
		GridDraw();
		DisplayStartandEnd(TargetPosRow, TargetPosCol);
		DrawBot();
		sleep(100);
		eraseDisplay();
	}


	//============================================================================================
	//array cancellation
	int first_index = 0;
	int second_index = 1;
	int first_value = 0;
	int second_value = 1;//seet as one so it doesn't bother while loop
	int index = 0;
	int index_new = 0;

	/*
	This while loop uses two pointers to find out the testing values then eliminates it if the difference of the two is 2.
	Values are eliminated by setting it to 10
	The first pointer is always set first before the second is set, and the second is always in front of the first.

	at the end, the array should contain non-eliminated values, or 10 or 0.
	*/
	while(second_value!=0)
	{
		if (position_array[first_index] != 10)//first pointer not pointing at eliminated value
		{
			first_value = position_array[first_index];//set first value
			second_index = first_index+1;//make sure second pointer is always ahead by one to avoid overlapping
			while(position_array[second_index]==10)
			{
				second_index++;//increment until you find a not eliminated value
			}
			second_value = position_array[second_index];

			//once you find the two non eliminated values, find if they need to be eliminated
			if(first_value-second_value == 2 && second_value!=0)	// * modified by Bowen
			{
				position_array[first_index]=10;
				position_array[second_index]=10;
				first_index = 0;
				second_index = 1;
			}
			else if (second_value-first_value == 2 && second_value!=0)	// * modified by Bowen
			{
				position_array[first_index]=10;
				position_array[second_index]=10;
				first_index = 0;
				second_index = 1;
			}
			else //if the two values' difference is not 2, increment both pointer to the next one.
			{
				first_index++;
				second_index++;
			}
		}
		else
		{
			first_index++;
		}
	}



	/*
	From the previuos step, the array is broken down to meaningful values or 0 or 10.
	This part is to copy over the meaningful values into a new array that will tell the robot the fastest route.
	*/
	while(position_array[index]!=0) // copy the left over ones into a new array
	{
		if(position_array[index]!=10){
			newArray[index_new] = position_array[index];
			index_new++;
			index++;
		}
		else
		{
			index++;//found a 10, increment old index and not the new index
		}
	}
	//=============================================================


	return newArray;
}

//=====================================================================

//setting all the walls to zero first
void GridInit(){
	for(int i=0;i<4;i++){
		for(int j=0;j<6;j++){
			Grid[i][j].NorthWall=0;
			Grid[i][j].EastWall=0;
			Grid[i][j].WestWall=0;
			Grid[i][j].SouthWall=0;
		}
	}
}
//=====================================================================
// setting all the walls according to maze
void WallGen(){
	Grid[0][0].NorthWall = 1;
	Grid[0][0].EastWall = 0;
	Grid[0][0].SouthWall = 1;
	Grid[0][0].WestWall = 1;

	Grid[1][0].NorthWall = 1;
	Grid[1][0].EastWall = 0;
	Grid[1][0].SouthWall = 1;
	Grid[1][0].WestWall = 1;

	Grid[2][0].NorthWall = 0;
	Grid[2][0].EastWall = 0;
	Grid[2][0].SouthWall = 1;
	Grid[2][0].WestWall = 1;

	Grid[3][0].NorthWall = 1;
	Grid[3][0].EastWall = 1;
	Grid[3][0].SouthWall = 0;
	Grid[3][0].WestWall = 1;

	// -------2---------
	Grid[0][1].NorthWall = 0;
	Grid[0][1].EastWall = 1;
	Grid[0][1].SouthWall = 1;
	Grid[0][1].WestWall = 0;

	Grid[1][1].NorthWall = 1;
	Grid[1][1].EastWall = 0;
	Grid[1][1].SouthWall = 0;
	Grid[1][1].WestWall = 0;

	Grid[2][1].NorthWall = 0;
	Grid[2][1].EastWall = 1;
	Grid[2][1].SouthWall = 1;
	Grid[2][1].WestWall = 0;

	Grid[3][1].NorthWall = 1;
	Grid[3][1].EastWall = 0;
	Grid[3][1].SouthWall = 0;
	Grid[3][1].WestWall = 1;

	// -------3--------

	Grid[0][2].NorthWall = 1;
	Grid[0][2].EastWall = 0;
	Grid[0][2].SouthWall = 1;
	Grid[0][2].WestWall = 1;

	Grid[1][2].NorthWall = 0;
	Grid[1][2].EastWall = 1;
	Grid[1][2].SouthWall = 1;
	Grid[1][2].WestWall = 0;

	Grid[2][2].NorthWall = 0;
	Grid[2][2].EastWall = 0;
	Grid[2][2].SouthWall = 0;
	Grid[2][2].WestWall = 1;

	Grid[3][2].NorthWall = 1;
	Grid[3][2].EastWall = 1;
	Grid[3][2].SouthWall = 0;
	Grid[3][2].WestWall = 0;

	// -------4--------
	Grid[0][3].NorthWall = 0;
	Grid[0][3].EastWall = 0;
	Grid[0][3].SouthWall = 1;
	Grid[0][3].WestWall = 0;

	Grid[1][3].NorthWall = 0;
	Grid[1][3].EastWall = 0;
	Grid[1][3].SouthWall = 0;
	Grid[1][3].WestWall = 1;

	Grid[2][3].NorthWall = 0;
	Grid[2][3].EastWall = 1;
	Grid[2][3].SouthWall = 0;
	Grid[2][3].WestWall = 0;

	Grid[3][3].NorthWall = 1;
	Grid[3][3].EastWall = 0;
	Grid[3][3].SouthWall = 0;
	Grid[3][3].WestWall = 1;

	// -------5--------
	Grid[0][4].NorthWall = 1;
	Grid[0][4].EastWall = 1;
	Grid[0][4].SouthWall = 1;
	Grid[0][4].WestWall = 0;

	Grid[1][4].NorthWall = 1;
	Grid[1][4].EastWall = 0;
	Grid[1][4].SouthWall = 1;
	Grid[1][4].WestWall = 0;

	Grid[2][4].NorthWall = 0;
	Grid[2][4].EastWall = 0;
	Grid[2][4].SouthWall = 1;
	Grid[2][4].WestWall = 1;

	Grid[3][4].NorthWall = 1;
	Grid[3][4].EastWall = 1;
	Grid[3][4].SouthWall = 0;
	Grid[3][4].WestWall = 0;

	// ------6--------

	Grid[0][5].NorthWall = 0;
	Grid[0][5].EastWall = 1;
	Grid[0][5].SouthWall = 1;
	Grid[0][5].WestWall = 1;

	Grid[1][5].NorthWall = 1;
	Grid[1][5].EastWall = 1;
	Grid[1][5].SouthWall = 0;
	Grid[1][5].WestWall = 0;

	Grid[2][5].NorthWall = 0;
	Grid[2][5].EastWall = 1;
	Grid[2][5].SouthWall = 1;
	Grid[2][5].WestWall = 0;

	Grid[3][5].NorthWall = 1;
	Grid[3][5].EastWall = 1;
	Grid[3][5].SouthWall = 0;
	Grid[3][5].WestWall = 1;

}
//=====================================================================
//drawing wall
void GridDraw(){
	int XStart=0;
	int YStart=0;
	int XEnd  =0;
	int YEnd  =0;
	for(int i=0;i<4;i++){
		for(int j=0;j<6;j++){
			if(Grid[i][j].NorthWall==1){
				XStart= j   *ScreenWidth/6;
				YStart=(i+1)*ScreenHeight/4;
				XEnd  =(j+1)*ScreenWidth/6;
				YEnd  =(i+1)*ScreenHeight/4;
				drawLine(XStart,YStart,XEnd,YEnd);
			}
			if (Grid[i][j].EastWall==1){
				XStart=(j+1)*ScreenWidth/6;
				YStart=(i)*ScreenHeight/4;
				XEnd  =(j+1)*ScreenWidth/6;
				YEnd  =(i+1)*ScreenHeight/4;
				drawLine(XStart,YStart,XEnd,YEnd);
			}
			if (Grid[i][j].WestWall==1){
				XStart= j   *ScreenWidth/6;
				YStart=(i)*ScreenHeight/4;
				XEnd  =(j)*ScreenWidth/6;
				YEnd  =(i+1)*ScreenHeight/4;
				drawLine(XStart,YStart,XEnd,YEnd);
			}
			if(Grid[i][j].SouthWall==1){
				XStart= j   *ScreenWidth/6;
				YStart=(i)*ScreenHeight/4;
				XEnd  =(j+1)*ScreenWidth/6;
				YEnd  =(i)*ScreenHeight/4;
				drawLine(XStart,YStart,XEnd,YEnd);
			}
		}
	}
}
//=====================================================================
void checkRight(){
	RobotDirection++;
	if(RobotDirection==4){
		RobotDirection=0;
	}
}
//=====================================================================
void checkLeft(){
	RobotDirection--;
	if(RobotDirection==-1){
		RobotDirection=3;
	}
}
//=====================================================================
void checkCurrentPos(){
	switch(RobotDirection){
	case 0: CurrentPosRow++; break; // Facing North
	case 1: CurrentPosCol++; break; // Facing East
	case 2: CurrentPosRow--; break; // Facing South
	case 3: CurrentPosCol--; break; // Facing West
	default: break;
	}
}
//=====================================================================
void DrawBot(){
	int RobotXpixelPos=0;
	int RobotYpixelPos=0;

	if(CurrentPosCol==0){
		RobotXpixelPos=ScreenWidth/12;
	}
	else{
		RobotXpixelPos=(2*CurrentPosCol+1)*ScreenWidth/12;
	}

	if(CurrentPosRow==0){
		RobotYpixelPos=ScreenHeight/8;
	}
	else{
		RobotYpixelPos=(2*CurrentPosRow+1)*ScreenHeight/8;
	}

	switch(RobotDirection){
	case 0: displayStringAt(RobotXpixelPos,RobotYpixelPos,"^");	break; // Facing North
	case 1: displayStringAt(RobotXpixelPos,RobotYpixelPos,">"); break; // Facing East
	case 2: displayStringAt(RobotXpixelPos,RobotYpixelPos,"V"); break; // Facing South
	case 3: displayStringAt(RobotXpixelPos,RobotYpixelPos,"<"); break; // Facing West
	default: break;
	}
}

//=====================================================================
int Solver(){
	checkRight();
	//no wall on the right of the robot
	if(CheckWall()==0){
		checkCurrentPos();
		return RobotDirection; // exit the Solver function
	}

	//no wall in front of the robot
	checkLeft();
	if(CheckWall()==0){
		checkCurrentPos();
		return RobotDirection; // exit the Solver function
	}

	checkLeft();
	//no wall on the left of the robot
	if(CheckWall()==0){
		checkCurrentPos();
		return RobotDirection; // exit the Solver function
	}

	checkLeft();
	//no wall behind the robot
	if(CheckWall()==0){
		checkCurrentPos();
		return RobotDirection; // exit the Solver function
	}
	return -1; // Should never arrive here
}

//=====================================================================
int CheckWall(){
	int WallStatus=0; // 0 = No Wall, 1= Wall

	switch(RobotDirection){
	case 0:
		if(Grid[CurrentPosRow][CurrentPosCol].NorthWall==1){
			WallStatus=1;
		}
		else{
			WallStatus=0; // exit the CheckWall function, Indicate Wall
		}
		break;
	case 1:
		if(Grid[CurrentPosRow][CurrentPosCol].EastWall==1){
			WallStatus=1;
		}
		else{
			WallStatus=0; // exit the CheckWall function, Indicate Wall
		}
		break;
	case 2:
		if(Grid[CurrentPosRow][CurrentPosCol].SouthWall==1){
			WallStatus=1;
		}
		else{
			WallStatus=0; // exit the CheckWall function, Indicate Wall
		}
		break;
	case 3:
		if(Grid[CurrentPosRow][CurrentPosCol].WestWall==1){
			WallStatus=1;
		}
		else{
			WallStatus=0; // exit the CheckWall function, Indicate Wall
		}
		break;
	default: break;
	}
	return WallStatus;
}
//=====================================================================
void DisplayStartandEnd(int TargetPosRow, int TargetPosCol){
	int XpixelPos=0;
	int YpixelPos=0;

	int StartPosRow = CurrentPosRow;
	int StartPosCol = CurrentPosCol;

	if(StartPosCol==0){
		XpixelPos=ScreenWidth/12;
	}
	else{
		XpixelPos=(2*StartPosCol+1)*ScreenWidth/12;
	}

	if(StartPosRow==0){
		YpixelPos=ScreenHeight/8;
	}
	else{
		YpixelPos=(2*StartPosRow+1)*ScreenHeight/8;
	}
	displayStringAt(XpixelPos,YpixelPos,"S");

	if(TargetPosCol==0){
		XpixelPos=ScreenWidth/12;
	}
	else{
		XpixelPos=(2*TargetPosCol+1)*ScreenWidth/12;
	}

	if(TargetPosRow==0){
		YpixelPos=ScreenHeight/8;
	}
	else{
		YpixelPos=(2*TargetPosRow+1)*ScreenHeight/8;
	}
	displayStringAt(XpixelPos,YpixelPos,"E");
}

void turnRight(){
	resetGyro(S3);
	resetMotorEncoder(motorA);
	resetMotorEncoder(motorB);
	wait1Msec(1000);
	int degrees = 90;
	while(abs(SensorValue[myGyro]) < degrees){
		// continue turning
		motor[rightMotor] = -25;
		motor[leftMotor] = 25;
	}
	// add a brake to stop drift
	motor[rightMotor] = 5;
	motor[leftMotor] = -5;
	wait1Msec(250);
	motor[rightMotor] = 0;
	motor[leftMotor] = 0;
	resetGyro(S3);
}

void turnLeft(){
	resetGyro(S3);
	resetMotorEncoder(motorA);
	resetMotorEncoder(motorB);
	wait1Msec(1000);
	int degrees = 90;
	while(abs(SensorValue[myGyro]) < degrees){
		// continue turning
		motor[rightMotor] = 25;
		motor[leftMotor] = -25;
	}
	// add a brake to stop drift
	motor[rightMotor] = -5;
	motor[leftMotor] = 5;
	wait1Msec(250);
	motor[rightMotor] = 0;
	motor[leftMotor] = 0;
	resetGyro(S3);
}

// Description: Updaing the coordinates of the robot.
void robotCurrentPos(){
	switch(RobotDirection){
	case 1: RobotCurrentRow++; break;
	case 2: RobotCurrentCol++; break;
	case 3: RobotCurrentRow--; break;
	case 4: RobotCurrentCol--; break;
	default: break;
	}
}

// ... Under Development ... //
void moveForward(){
	resetMotorEncoder(motorA);
	resetMotorEncoder(motorB);
	//if(checkFrontWall() == true){
	//	while(!(SensorValue(leftTouch) == 1 && SensorValue(rightTouch) == 1)){
	//		motor[motorA] = 25;
	//		motor[motorB] = 25;
	//	}
	//	resetMotorEncoder(motorA);
	//	resetMotorEncoder(motorB);
	//	setMotorTarget(motorA, -70, 25);
	//	setMotorTarget(motorB, -70, 25);
	//	waitUntilMotorStop(motorA);
	//	waitUntilMotorStop(motorB);
	//}
	//else{
		moveMotorTarget(motorA, 650, 25);
		moveMotorTarget(motorB, 650, 25);
		waitUntilMotorStop(motorA);
		waitUntilMotorStop(motorB);
	//}
	robotCurrentPos();
}

// -------------- Moving Physical Robot --------------- //
void movingToPath(int* shortestPath){
	RobotDirection = 1;
	int i = 0;
	while(shortestPath[i] != 0){
		if(RobotDirection - shortestPath[i] == -1 || RobotDirection - shortestPath[i] == 3 ){
			RobotDirection = shortestPath[i];
			turnRight();
			moveForward();
		}
		else if(RobotDirection - shortestPath[i] == 1 || RobotDirection - shortestPath[i] == -3 ){
			RobotDirection = shortestPath[i];
			turnLeft();
			moveForward();
		}
		else if(RobotDirection == shortestPath[i]){
			moveForward();
		}
		i++;
	}
}

bool checkFrontWall(){
	switch(RobotDirection){
        case 1:{
    		if(RobotDirection == 1 && Grid[RobotCurrentRow+1][RobotCurrentCol].NorthWall == 1){
    			return true;
    		}
    		break;
    	}
        case 2:{
        	if(RobotDirection == 2 && Grid[RobotCurrentRow][RobotCurrentCol+1].EastWall == 1){
        		return true;
        	}
        	break;
        }
        case 3:{
        	if(RobotDirection == 3 && Grid[RobotCurrentRow-1][RobotCurrentCol].SouthWall == 1){
        		return true;
        	}
        	break;
        }
        case 4:{
        	if(RobotDirection == 4 && Grid[RobotCurrentRow][RobotCurrentCol-1].WestWall == 1){
        		return true;
        	}
        	break;
        }
	    default: break;
	}
	return false;
}

void reset(){
	resetMotorEncoder(motorA);	// wheel
	resetMotorEncoder(motorB);	// wheel
	resetMotorEncoder(motorC);	// lift motor
}

void stackBlock(){
	// lift up to max height
	sleep(1000);
	setMotorTarget(motorC, 9000, 100);
	waitUntilMotorStop(motorC);

	// go forward
	reset();
	sleep(1000);
	setMotorTarget(motorA, 350, 25);
	setMotorTarget(motorB, 350, 25);
	waitUntilMotorStop(motorA);
	waitUntilMotorStop(motorB);

	// lift down to drop off height
	reset();
	sleep(1000);
	setMotorTarget(motorC, -3000, 100);
	waitUntilMotorStop(motorC);

	// back up
	reset();
	sleep(1000);
	setMotorTarget(motorA, -350, 25);
	setMotorTarget(motorB, -350, 25);
	waitUntilMotorStop(motorA);
	waitUntilMotorStop(motorB);

	// lift down to lowest
	reset();
	sleep(1000);
	setMotorTarget(motorC, -11000, 100);
	waitUntilMotorStop(motorC);

	// go forward to get both blocks
	reset();
	sleep(1000);
	setMotorTarget(motorA, 350, 25);
	setMotorTarget(motorB, 350, 25);
	waitUntilMotorStop(motorA);
	waitUntilMotorStop(motorB);

	//lift up to max height
	reset();
	sleep(1000);
	setMotorTarget(motorC,14000, 100);
	waitUntilMotorStop(motorC);

	// return to middle of cell
	reset();
	sleep(1000);
	setMotorTarget(motorA, -70, 25);
	setMotorTarget(motorB, -70, 25);
	waitUntilMotorStop(motorA);
	waitUntilMotorStop(motorB);
}

void takeBlock(){
	//go forward
	reset();
	sleep(1000);
	setMotorTarget(motorA, 350, 25);
	setMotorTarget(motorB, 350, 25);
	waitUntilMotorStop(motorA);
	waitUntilMotorStop(motorB);

	//lift up to max height
	reset();
	sleep(1000);
	setMotorTarget(motorC,14000, 100);
	waitUntilMotorStop(motorC);

	//back up
	reset();
	sleep(1000);
	setMotorTarget(motorA, -70, 25);
	setMotorTarget(motorB, -70, 25);
	waitUntilMotorStop(motorA);
	waitUntilMotorStop(motorB);
}

void faceNorth(){
	switch(RobotDirection){
		case 1:{
			RobotDirection = 1;
			break;
		}
		case 2:{
			turnLeft();
			RobotDirection = 1;
			break;
		}
		case 3:{
			turnLeft();
			turnLeft();
			RobotDirection = 1;
			break;
		}
		case 4:{
			turnRight();
			RobotDirection = 1;
			break;
		}
		default: break;
	}
}