Selvmanøvrerende Mindstorms robot

Robotten kan fjernstyres og er i stand til at scanne for forhindringer og undvige. Den kan desuden informere grundigt på displayet.

#pragma config(Sensor, S1,     SoundSensor,    sensorSoundDB)
#pragma config(Sensor, S3,     SonarSensor,    sensorSONAR)
#pragma config(Motor,  motorA,          Arm,           tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  motorB,          Right,         tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  motorC,          Left,          tmotorNXT, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//////////// Defining global vars ////////////

int SoundSensorSetting;   // Create a boolean variable (true/false).
int Distance_cm = 35;  // Create integrier variable (16 bit, whole numbers only).
string text0;
string text1;
string text2;
string text3;
int RandomNr;   // Create variable "RandomNr".
int Run_AI2_TurnScan;
int DriveSpeed;

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

void drive(bool DriveMode)
{

	if (DriveMode == true)
	{
		motor[Right] = DriveSpeed;
		motor[Left] = DriveSpeed;
	}

	if (DriveMode == false)
	{
		motor[Right] = 0;
		motor[Left] = 0;
	}

}

void AI2_TurnScan()
{
	if (Run_AI2_TurnScan == 1)
	{

		int ScanDistance_Left;
		int ScanDistance_Right;

		drive(false);

		// Scan the area.
		nMotorEncoder[Arm] = 0;

		// Look to the right.
		while (nMotorEncoder[Arm] < 90)
		{
			motor[Arm] = 20;   // Speed of the motor, where the sonar sensor is mounted.
		}

		motor[Arm] = 0;
		ScanDistance_Right = SensorValue[SonarSensor];
		wait1Msec(1000);

		// Look to the left.
		while (nMotorEncoder[Arm] > -90)
		{
			motor[Arm] = -20; // Speed of the motor, where the sonar sensor is mounted.
		}

		motor[Arm] = 0;
		ScanDistance_Left = SensorValue[SonarSensor];
		wait1Msec(1000);

		// Look forward.
		while (nMotorEncoder[Arm] < 0)
		{
			motor[Arm] = 10;
		}

		motor[Arm] = 0;

		// Check scan values in compare to variable "Distance_cm" (35cm) and mark them as possible turn direction or not.
		int TurnDirection;

		if ((ScanDistance_Left < Distance_cm) && (ScanDistance_Right < Distance_cm))   // If: Can't turn left nor right.
		{
			TurnDirection = 0;
		}

		if ((ScanDistance_Left > Distance_cm) && (ScanDistance_Right > Distance_cm))   // If: Can turn left and right.
		{
			TurnDirection = 1;
		}

		if ((ScanDistance_Left > Distance_cm) && (ScanDistance_Right < Distance_cm))   // If: Can turn left, but not right.
		{
			TurnDirection = 2;
		}

		if ((ScanDistance_Left < Distance_cm) && (ScanDistance_Right > Distance_cm))   // If: Can turn right, but not left.
		{
			TurnDirection = 3;
		}

		wait1Msec(100);

		// Debug, showing scan results.
		string DebugText1 = "TurnDir";
		nxtDisplayCenteredTextLine(6, "%s, %d", DebugText1, TurnDirection);
		wait1Msec(250);

		// Turn appropriately. Pick random if both applies. Drive back and turn around if none applies.
		//		int Rotate360Timer = 0;

		switch(TurnDirection)
		{

		case 0:   // If: Can't turn left nor right.

			nMotorEncoder[Right] = 0;
			nMotorEncoder[Left] = 0;

			while ((nMotorEncoder[Left] > -500) && (nMotorEncoder[Right] > -500))
			{
				motor[Left] = -35;
				motor[Right] = -35;
			}

			drive(false);
			nMotorEncoder[Right] = 0;
			nMotorEncoder[Left] = 0;
			wait1Msec(200);

			while (nMotorEncoder[Right] > -333)
			{
				// Turn 180.
				motor[Right] = -20;
				motor[Left] = 20;
			}

			drive(false);
			break;

		case 1:   // If: Can turn left and right.

			nMotorEncoder[Right] = 0;
			nMotorEncoder[Left] = 0;

			RandomNr = random(2);   // Set "RandomNr" variable to be random. "2" is the maximum number (1 or 2, 0 gets ignored).
			wait1Msec(100);

			// Debug, showing random number results.
			string DebugText2 = "RandomNr";
			nxtDisplayCenteredTextLine(7, "%s, %d", DebugText2, RandomNr);

			switch(RandomNr)
			{

			case 1:   // If the random number is "0".

				while (nMotorEncoder[Right] < 160)
				{
					// Makes the robot turn left.
					motor[Right] = 10;
					motor[Left] = -10;
				}

				drive(false);
				break;

			case 2:   // If the random number is "1".

				while (nMotorEncoder[Left] < 160)
				{
					// Makes the robot turn right.
					motor[Right] = -10;
					motor[Left] = 10;
				}

				drive(false);
				break;
			}

			break;

		case 2:   // If: Can turn left, but not right.

			nMotorEncoder[Right] = 0;
			nMotorEncoder[Left] = 0;

			while (nMotorEncoder[Right] < 160)
			{
				// Makes the robot turn left.
				motor[Right] = 10;
				motor[Left] = -10;
			}

			drive(false);
			break;

		case 3:   // If: Can turn right, but not left.

			nMotorEncoder[Right] = 0;
			nMotorEncoder[Left] = 0;

			while (nMotorEncoder[Left] < 160)
			{
				// Makes the robot turn right.
				motor[Right] = -10;
				motor[Left] = 10;
			}

			drive(false);
			break;
		}

	}

}

void DrawUI(int UI_ID)
{

	if (UI_ID == 1)
	{
		// Print the text of the "sensor settings" page on the display.
		text0 = "SETTINGS";
		text1 = "Allow the sound";
		text2 = "sensor to stop";
		text3 = "the program?";
		nxtDisplayCenteredBigTextLine(1, text0);   // Prints the content of "text0" on line 1 as a big, centered text.
		nxtDisplayCenteredTextLine(3, text1);   // Prints the content of "text1" on line 3 as the default centered text.
		nxtDisplayCenteredTextLine(4, text2);
		nxtDisplayCenteredTextLine(5, text3);
		nxtDisplayStringAt(64, 10, "Deny");   // Prints "Deny" at X(67), Y(10).
		nxtDisplayStringAt(9, 10, "Accept");

		// Draws "The UI" of the "sensor settings" page.
		nxtDrawLine(0, 14, 99, 14);   // Draws the UI horizontal line near the middle of the display. Syntax: From [x,y] to [x,y].
		nxtDrawLine(50, 14, 50, 0);   // Draws the UI vertical line near the middle of the display.
		nxtDrawRect(0, 0, 99, 63);   // Draws the UI full screen frame of the display.
	}

	if (UI_ID == 3)
	{
		// Print the text of the "driving speed settings" page on the display.
		text0 = "SETTINGS";
		text1 = "Set the default";
		text2 = "driving speed.";
		nxtDisplayCenteredBigTextLine(1, text0);   // "Settings".
		nxtDisplayCenteredTextLine(3, text1);
		nxtDisplayCenteredTextLine(4, text2);
		nxtDisplayStringAt(76, 12, "+10");
		nxtDisplayStringAt(7, 12, "-10");

		// Draws the UI of the "driving speed settings" page on the display.
		nxtDrawLine(0, 18, 99, 18);   // Horizontal line.
		nxtDrawLine(35, 0, 35, 18);   // Left vertical line 1.
		nxtDrawLine(31, 0, 31, 18);   // Left vertical line 2.
		nxtDrawLine(64, 0, 64, 18);   // Right vertical line 1.
		nxtDrawLine(68, 0, 68, 18);   // Right vertical line 2.
		nxtDrawRect(0, 0, 99 , 63);   // Border rectangle.
	}

	if (UI_ID == 4)
	{
		// Prints the text of the "starting program" page on the display.
		text0 = "STARTING";
		text1 = "PROGRAM";
		nxtDisplayCenteredBigTextLine(1, text0);
		nxtDisplayCenteredBigTextLine(3, text1);

		// Draws the "UI" for the "starting program" page.
		nxtDrawLine(0, 21, 99, 21);   // Draws the vertical line near the middle of the display.
		nxtDrawRect(40, 3, 60, 18);   // Draws the UI small frame around the count-down timer.
		nxtDrawRect(0, 0, 99, 63);   // Draws the UI full screen frame of the display.
	}

}

task UILoop()
{
	bNxtLCDStatusDisplay = true;   // Boolean variable. The "NXT top status line" Will display on user-drawn LCD screens.
	nxtDrawRect(0, 0, 99, 54);   // Draws the UI full screen frame of the display.

	while (true)
	{
		text0 = "Running.  ";
		nxtDisplayCenteredTextLine(4, text0);
		nxtDrawRect(0, 0, 99, 54);   // Draws the UI full screen frame of the display.
		wait1Msec(1000);

		text0 = "Running.. ";
		nxtDisplayCenteredTextLine(4, text0);
		nxtDrawRect(0, 0, 99, 54);   // Draws the UI full screen frame of the display.
		wait1Msec(1000);

		text0 = "Running...";
		nxtDisplayCenteredTextLine(4, text0);
		nxtDrawRect(0, 0, 99, 54);   // Draws the UI full screen frame of the display.
		wait1Msec(1000);
	}

}

void ResetRobot()
{
	Run_AI2_TurnScan = 0;
	StopTask(UILoop);
	drive(false);   // Stop driving.
	wait1Msec(100);

	if (nMotorEncoder[Arm] < 0)   // If the "arm" motor is below it's 0 position.
	{

		while (nMotorEncoder[Arm] < 0)   // Run the motor until the motor reaches it's "0" position (looking forward).
		{
			motor[Arm] = 10;
		}

		motor[Arm] = 0;
	}

	if (nMotorEncoder[Arm] > 0)   // If the "arm" motor is above it's 0 position.
	{

		while (nMotorEncoder[Arm] > 0)   // Run the motor until the motor reaches it's "0" position (looking forward).
		{
			motor[Arm] = -10;
		}

		motor[Arm] = 0;
	}

	wait1Msec(100);
	StopAllTasks();   // Stops the whole program.
}

task StopProgramCheck()
{
	// Clicking the "EXIT" button 2 times will terminate program, meaning 1 button-click is available for the program to use.
	nNxtExitClicks = 2;

	while (true)
	{
		// If the Sound sensor has been enabled and the detects sound above 65%..
		// Or if button 0 (gray rectangle button) has been pressed...

		if ((nNxtButtonPressed == 0) || ((SoundSensorSetting == true) && (SensorValue[SoundSensor] > 65)))
		{
			ResetRobot();
		}

		wait1Msec(100);
	}

}

task main()
{
	wait1Msec(100);

	// Print the the "sensor settings" page on the display.
	DrawUI(1);

	// The configuration of the "sensor settings" page.
	bool ExitSettingsMenu = false;

	while (ExitSettingsMenu == false)
	{

		if (nNxtButtonPressed == 2)   // If "left arrow" button has been pressed.
		{
			SoundSensorSetting = 1;
			ExitSettingsMenu = true;
		}

		if (nNxtButtonPressed == 1)   // If "right arrow" button has been pressed.
		{
			SoundSensorSetting = 0;
			ExitSettingsMenu = true;
		}

		if (nNxtButtonPressed == 3)   //If "orange" button has been pressed.
		{
			ExitSettingsMenu = false;
		}

		wait1Msec(100);
	}

	eraseDisplay();
	wait1Msec(300);

	// Prints the "drive speed settings" page on the display.
	int DriveSpeedSetting = 100;
	text3 = DriveSpeedSetting;
	nxtDisplayStringAt(42, 12, text3);
	DrawUI(3);

	// The configuration of the "drive speed settings" page.
	ExitSettingsMenu = false;

	while (ExitSettingsMenu == false)
	{

		if (nNxtButtonPressed == 1)   // Right.
		{

			if (DriveSpeedSetting < 100)
			{

				DriveSpeedSetting = DriveSpeedSetting + 10;
				text3 = DriveSpeedSetting;
				eraseDisplay();
				wait1Msec(10);

				// If DriveSpeedSetting is 2 digits long.
				if ((DriveSpeedSetting > 9) && (DriveSpeedSetting < 99))
				{
					nxtDisplayStringAt(45, 12, text3);
				}

				// If DriveSpeedSetting is 1 digit long.
				if (DriveSpeedSetting < 1)
				{
					nxtDisplayStringAt(47, 12, text3);
				}

				// If DriveSpeedSetting is 3 digits long.
				if (DriveSpeedSetting > 99)
				{
					nxtDisplayStringAt(42, 12, text3);
				}

			}

			DrawUI(3);
			wait1Msec(300);
		}

		if (nNxtButtonPressed == 2)   // Left.
		{

			if (DriveSpeedSetting > 0)
			{

				DriveSpeedSetting = DriveSpeedSetting - 10;
				text3 = DriveSpeedSetting;
				eraseDisplay();
				wait1Msec(10);

				// If DriveSpeedSetting is 2 digits long.
				if ((DriveSpeedSetting > 9) && (DriveSpeedSetting < 99))
				{
					nxtDisplayStringAt(45, 12, text3);
				}

				// If DriveSpeedSetting is 1 digit long.
				if (DriveSpeedSetting < 1)
				{
					nxtDisplayStringAt(47, 12, text3);
				}

				if (DriveSpeedSetting > 99)
				{
					// If DriveSpeedSetting is 3 digits long.
					nxtDisplayStringAt(42, 12, text3);
				}

				DrawUI(3);
				wait1Msec(300);
			}

		}

		if (nNxtButtonPressed == 3)   // Orange.
		{
			DriveSpeed = DriveSpeedSetting;  // sets the drive speed set in above settings to be the defalt drive speed.
			ExitSettingsMenu = true;
		}

		wait1Msec(100);
	}

	eraseDisplay();
	wait1Msec(300);

	// Prints the "starting program" page on the display.
	DrawUI(4);

	// Prints the count-down timer.
	nxtDisplayStringAt(49, 13, "3");   // Displays "3" inside the count-down timer frame.
	wait1Msec(1000);
	nxtDisplayStringAt(49, 13, "2");   // Displays "2" inside the count-down timer frame.
	wait1Msec(1000);
	nxtDisplayStringAt(49, 13, "1");   // Displays "1" inside the count-down timer frame.
	wait1Msec(1000);
	nxtDisplayStringAt(49, 13, "0");   // Displays "0" inside the count-down timer frame.
	wait1Msec(750);

	eraseDisplay();
	wait1Msec(250);

	// Starts the task controlling the "running program" page on the display. This is running a seperate task due to its' "animated text".
	StartTask(UILoop);

	// Starts the actual program... >_<
	StartTask(StopProgramCheck);
	bFloatDuringInactiveMotorPWM = false;   // Makes sure the motor stops when unpowered, intead of "coasting".
	Run_AI2_TurnScan = 1;

	while (true)   // Infinite loop.
	{

		while (SensorValue[SonarSensor] > Distance_cm)
		{
			drive(true);   // Sets the motor A and B to drive at the speed provided by the "drive speed settings" page.
			wait1Msec(100);  // The robot waits 0.10 seconds till it checks the distance again (to reduce load).
		}

		AI2_TurnScan();
	}

}

Skriv et svar