Our Unfinished Code

 here

	#include <Adafruit_MotorShield.h>
	#include <Servo.h>
	#include <Wire.h>
	// Pin constants for the Ultrasonic sensor.
	const int pingPinOut = 8;
	const int pingPinIn = 8;
	// Constant Definitions
	const unsigned slowSpeed = 400;
	const unsigned fastSpeed = 800;
	const unsigned openSpace = 7;
	const unsigned turnTime = 1000;
	Adafruit_MotorShield AFMS = Adafruit_MotorShield();
	// Declares the left motor on the Adafruit M3 and right motor on M4.
	// Declares the Ultrasonic sensor as "myLooker".
	Adafruit_DCMotor *lMotor = AFMS.getMotor(2);
	Adafruit_DCMotor *rMotor = AFMS.getMotor(3);
	Servo myLooker;
	void setup()
	{
	Serial.begin(9600);
	// Declares the servo motor that the ultrasonic sensor is on to pin 9.
	myLooker.attach(9);
	AFMS.begin();
	}
	void loop()
	{
	delay(2000);
	bool found = SolveMaze();
	// If the goal of the maze was found, the robot will do a happy dance.
	if (found == true)
	{
	goLeft(100);
	goRight(100);
	goLeft(100);
	goRight(100);
	}
	}
	bool SolveMaze()
	{
	// Begins infinite loop to look for the maze goal.
	for (;;)
	{
	// Declares the step counter "i" and the character path array "path".
	unsigned i = 0;
	char path[50];
	// Assigns the variable "cm" to the distance to the wall that is calculated by the function "DistancePing".
	long cm = DistancePing();
	long x = cm;
	// Begins loop to move forward and put the letter "F" into the path array.
	// This continues for a preset distance.
	LookForward();
	path[i++] = 'F';
	goForward(slowSpeed);
	while (cm >= x - 15)
	{
	goForward(slowSpeed);
	cm = DistancePing();
	}
	// Determines if either the left or right direction is open.
	bool left = LookLeft();
	bool right = LookRight();
	// If Both are left and right are open the robot will determine if it is at the goal.
	if (left == true && right == true)
	{
	bool end = ChckForEnd();
	if (end == true)
	{
	// If it is at the goal it will begin to shorten the path array.
	char sPath[50];
	char one, two, three;
	unsigned index;
	for (index = 0; index <= i; index++)
	{
	one = path[index];
	two = path[index + 1];
	three = path[index + 2];
	if (one == 'L' && two == 'B' && three == 'R')
	sPath[index] = 'B';
	else if (one == 'L' && two == 'B' && three == 'F')
	sPath[index] = 'R';
	else if (one == 'R' && two == 'B' && three == 'L')
	sPath[index] = 'B';
	else if (one == 'F' && two == 'B' && three == 'L')
	sPath[index] = 'R';
	else if (one == 'F' && two == 'B' && three == 'F')
	sPath[index] == 'B';
	else if (one == 'L' && two == 'B' && three == 'L')
	sPath[index] == 'F';
	else
	sPath[index] = path[index];
	}
	// The robot will then return to the start of the maze.
	for (unsigned a = index; a >= 0; a--)
	{
	char step;
	step = sPath[a];
	if (step == 'F')
	goBack(slowSpeed);
	else if (step == 'R')
	goLeft(slowSpeed);
	else if (step == 'L')
	goRight(slowSpeed);
	}
	// The robot will wait and begin its speed run through the maze.
	delay(5000);
	for (unsigned b = 0; b <= index; b++)
	{
	if (sPath[b] == 'F')
	goForward(fastSpeed);
	else if (sPath[b] == 'L')
	goLeft;
	else if (sPath[b] == 'R')
	goRight(fastSpeed);
	else
	break;
	}
	return true;
	}
	}
	// If the robot is not at the goal and the left path is open, it will go left.
	// Otherwise if right is open it will go right.
	// If neither are open, it will turn around and go back.
	if (left == true)
	{
	goLeft(slowSpeed);
	path[i++] = 'L';
	}
	else if (right == true)
	{
	goRight(slowSpeed);
	path[i++] = 'R';
	}
	else
	{
	goBack(slowSpeed);
	path[i++] = 'B';
	}
	}
	}
	/*-------------- g o F o r w a r d ( ) ----------------------
	Purpose: To move the robot in the forward direction.
	*/
	void goForward(int speed)
	{
	delay(1000);
	lMotor->setSpeed(speed);
	rMotor->setSpeed(speed);
	lMotor->run(FORWARD);
	rMotor->run(FORWARD);
	delay(turnTime);
	lMotor->run(RELEASE);
	rMotor->run(RELEASE);
	}
	/*-------------- g o L e f t ( ) -------------------------
	Purpose: To turn the robot left.
	*/
	void goLeft(int speed)
	{
	delay(1000);
	lMotor->setSpeed(speed);
	rMotor->setSpeed(speed);
	lMotor->run(BACKWARD);
	rMotor->run(FORWARD);
	delay(turnTime);
	lMotor->run(RELEASE);
	rMotor->run(RELEASE);
	}
	/*---------------------- g o R i g h t ( ) ---------------------
	Purpose: To turn the robot right
	*/
	void goRight(int speed)
	{
	delay(1000);
	lMotor->setSpeed(speed);
	rMotor->setSpeed(speed);
	lMotor->run(FORWARD);
	rMotor->run(BACKWARD);
	delay(turnTime);
	lMotor->run(RELEASE);
	rMotor->run(RELEASE);
	}
	/*---------------------- g o B a c k ( ) ----------------------
	Purpose: to turn the robot 180 degrees
	*/
	void goBack(int speed)
	{
	delay(1000);
	lMotor->setSpeed(speed);
	rMotor->setSpeed(speed);
	lMotor->run(FORWARD);
	rMotor->run(BACKWARD);
	delay(turnTime);
	delay(turnTime);
	lMotor->run(RELEASE);
	rMotor->run(RELEASE);
	}
	/*-------------- D i s t a n c e P i n g ( ) ---------------------------
	Purpose: To determine the distance to the wall using the ultrasonic sensor.
	*/
	long DistancePing()
	{
	long duration, cm;
	// Pings a test pulse, then the measured one.
	pinMode(pingPinOut, OUTPUT);
	digitalWrite(pingPinOut, LOW);
	delayMicroseconds(2);
	digitalWrite(pingPinOut, HIGH);
	delayMicroseconds(10);
	digitalWrite(pingPinOut, LOW);
	// Read in the time it takes for the signal to travel and return.
	pinMode(pingPinIn, INPUT);
	duration = pulseIn(pingPinIn, HIGH);
	return (cm / 29 / 2);
	}
	/*----------------------- L o o k F o r w a r d ( ) -------------------------
	Purpose: Turns the servo so that the sensor is facing forward.
	*/
	void LookForward()
	{
	delay(1000);
	int x = 90;
	myLooker.write(x);
	}
	/*------------------------ L o o k L e f t ( ) ----------------------------
	Purpose: Determines if the left side of the robot is open.
	*/
	bool LookLeft()
	{
	delay(1000);
	long cM;
	myLooker.write(0);
	cM = DistancePing();
	if (cM >= openSpace)
	return true;
	else
	return false;
	}
	/*--------------------- L o o k R i g h t ( )------------------------------
	Purpose: Determines if the right side of the robot is open.
	*/
	bool LookRight()
	{
	delay(1000);
	long centiM;
	myLooker.write(180);
	centiM = DistancePing();
	if (centiM >= openSpace)
	return true;
	else
	return false;
	}
	/*----------------------- C h c k F o r E n d ( ) ---------------------------
	Purpose: Determines if the robot is at the goal of the maze.
	*/
	bool ChckForEnd()
	{
	delay(1000);
	long leftCenti, rightCenti;
	myLooker.write(135);
	rightCenti = DistancePing();
	myLooker.write(45);
	leftCenti = DistancePing();
	if (rightCenti >= openSpace && leftCenti >= openSpace)
	return true;
	else
	return false;
	}

view raw Micro-Mouse-Code-Unfinished hosted with ❤ by GitHub