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();
}
}