FLL 2013 All Missions
#pragma systemFile // this eliminates warning for "unreferenced" functions
//#pragma config(Sensor, S2, lightSensorB, sensorLightInactive)
//#pragma config(Sensor, S3, lightSensorA, sensorLightInactive)
//#pragma config(Sensor, S4, sonarSensor, sensorSONAR)
//#pragma config(Sensor, S4, colorSensor, sensorCOLORFULL)
//#pragma config(Sensor, S2, lightSensorB, sensorLightInactive)
//#pragma config(Sensor, S3, lightSensorA, sensorLightInactive)
//
//
/*--
Includes File: Library2013.c
Date: September 1, 2013
========================================================
Note to myself: Functions still needed: untilBump(); // 100ms delay
All sensors use variables to store and manipulate values.
However, not all sensors will be used on the robot at the same time.
Different configurations of robots may require different sets of sensors.
Initialize variables for any sensors which are NOT being used by the robot.
The sensors which are being used are defined using Config statements in the main program.
The main program will begin with Pragma statements such as:
#pragma config(Sensor, S1, touchSensor, sensorTouch)
#pragma config(Sensor, S3, lightSensorA, sensorLightActive)
In earlier versions of RobotC, these statements used to be at the top of the "include" file.
However, later versions of RobotC have required that the pragma statements appear at the top of the main c file.
I have left the #pragma statements at the top of this file for archival purposes and to preserve the field naming.
One important note, however, is that the the #pragma's are only defined for those sensors what are used on the robot.
This presents a problem in the Library in that the Library was written to be general enough to be used with all
NXT sensors that may appear on a Robot. Therefore, it is necessary to define as "variables" those variables which
are not able to be defined through the #pragma statements. In essence, each sensor not being used on the robot needs
to be defined as a variable below. I have defined all the sensors as variables below and simply comment out those that
are already defined through the #pragma statements above. Get it?
--*/
int colorSensor;
//int touchSensor;
//int sonarSensor;
int lightSensorB;
//int lightSensorA;
/////////////////////////////////////////////
// Internal constants used to write more readable code.
/*--
The next few sections of code are used to create (define) variables and constants.
These often are used to help make the code more readable. So, you will see common words used as variable names such as:
LEFTSIDE, CLOCKWISE, sideOfLine, lineIsAtLevel, ...
In general, constants are shown in all upper case letters while variables are shown in mixed case.
If you look at the variables and constants below, you may notice that some are polar opposites (RIGHT/LEFT; FORWARD/BACKWARD;
CLOCKWISE/COUNTERCLOCKWISE). In addition to being worded as opposites, they are also given values which are opposite: 1 vs. -1
These values make it easier to determine the proper value of formulas. For example, the direction of the robot may be stored
in a variable called "direction" and given a value of either FORWARD or BACKWARD. Thus the power given to a motor is equal to
PowerLevel * direction where the PowerLevel is a positive integer from 0 to 100 and the direction is either +1 or -1.
The motors themselves may be right-side up or up-side down in the design of the robot. We can store the "orientation" of
the motors in a variable called "orientation". Thus the power to a motor is now
equal to the PowerLevel * direction * orientation.
--*/
/////////////////////////////////////////////
// Variables used in Line Following
//
const int RIGHTSIDE = 1; // Line following constants used to tell which side of line to follow
const int MIDDLE = 0; // You tell the robot to follow the line on the Leftside, Rightside, or the Middle
const int LEFTSIDE = -1; // using functions such as: followLine(LEFTSIDE);
int sideOfLine = RIGHTSIDE; // Initialize a variable to keep track of which side of the line the robot is following
int lineIsAtLevel = 20; // Percentage of light that is used to determine presence of a line.
// If the percentage of light read by the light sensor is 20% or less than the
// highest light level, then I guess you are looking at a line.
// If the robot consistently misses the line, make sure you have calibrated the light sensor
/////////////////////////////////////////////
// Moving Constants
const int STOPPED = 0; // A 'constant' is a type of variable that does not change in value
const int FORWARD = 1; // General navigation used to keep track of which direction the robot is going
const int BACKWARD = -1; // These are declared as variables so they can be reversed for certain robot designs
int direction = STOPPED; // Initial direction of the robot is set to Stopped
int mDir = FORWARD; // Orientation of the motors. If motors are reversed, the config function will change this value
int aDir = FORWARD; // Orientation of the Accessory Motor
bool keepGoing = false; // Some functions allow for the robot to keep moving when true;
// If false, robot will stop after each action
/////////////////////////////////////////////
// Variables used in Turning
// When making turns,often the motor on the "outside" wheel for the turn is set as the master
// for purposes of determining degreesof rotation. For example, when moving forward, a
// clockwise turn uses one motor for the "outside" wheel while a turn counter-clockwise
// uses the other motor for the "outside" wheel. While separate blocks of code could be
// written in the turning functions to handle this, a simpler method is to use variables.
int mMotor = motorB; // The "Main" motor is used to simplify code. If the motors are upside down, then the main motor may need to be changed.
int sMotor = motorC; // The "Secondary" motor is used to simplify code. If the motors are upside down, then the main motor may need to be changed.
const int CLOCKWISE = 1;
const int COUNTERCLOCKWISE = -1;
//
//
/////////////////////////////////////////////
// Variables used for Power
int powerLevel = 0; // Level of power used by all motor actions
int powerLevelRev = 0; // Level of power used by all Reverse motor actions
int powerLevelAccessory = 20; // Level of power used by the accessory motor
//
//
/////////////////////////////////////////////
// Variables to handle possible slack in the gear.
int lastDirection = direction; // Last movement direction of the robot
bool addToDistance = false; // A flag indicating whether extra distance needs to be added to the criteria
float motorError[] = {0, 0, 0, 0};
//////////////////////////////////////////////////////////////////////
//
// Define Primitive functions that may be referenced elsewhere
//
// The startUp() function resets the encoder to help prevent the encoder value from exceeding the range.
// Also, the waitForOrangeButton() also resets the counter since the user may have picked up the robot and moved it.
void resetRotationSensor(tMotor thisMotor) {
nSyncedMotors = synchNone; // De-sync the motors. The slave motor encoder cannot be altered while synced.
nMotorEncoder[thisMotor] = 0;
if(thisMotor==motorA) { motorError[1]=0; }
if(thisMotor==motorB) { motorError[2]=0; }
if(thisMotor==motorC) { motorError[3]=0; }
}
void resetRotationSensor(int motor1, int motor2) {
resetRotationSensor(motor1);
resetRotationSensor(motor2);
}
void resetRotationSensor(int motor1, int motor2, int motor3) {
resetRotationSensor(motor1, motor2);
resetRotationSensor(motor3);
}
void synchMotors(int master) {
if(master == (int) motorB) {
//PlaySoundFile("OK.rso");
nSyncedMotors = synchBC;
}
if(master == (int) motorC) {
//PlaySoundFile("OK.rso");
nSyncedMotors = synchCB;
}
}
void display(int lineNum, string *sgb) { // Display a message on a certain line
nxtDisplayTextLine(lineNum, "%s", sgb);
}
void waitForOrangeButton() {
display(6,"*Press Orange Btn"); // Notify User
display(7,"* to continue. *");
while(nNxtButtonPressed != kEnterButton) { // Wait Here
wait1Msec(10);
}
display(6," *"); // Clear the message
display(7,"* *");
resetRotationSensor(motorB, motorC);
}
void wait(float time) {
wait1Msec(time * 1000);
}
void waitForTouchSensor() {
display(6,"*Press Touch Snsr"); // Notify User
display(7,"* to continue. *");
while(SensorValue(touchSensor) == 0)
{ // Wait Here
wait1Msec(10);
}
display(6," *"); // Clear the message
display(7,"* *");
resetRotationSensor(motorB, motorC);
}
// This is an unusual function. There are some parts of the code that store an error value for each motor using an array.
// The array elements are referenced using 1, 2, or 3. However, the motors identified as motorA, motorB, and motorC are
// not assigned numeric values in the same way. Therefore, it is necessary to create a function to convert from motor value
// to integer value.
int convertMotorToInt(tMotor thisMotor) {
if(tMotor==motorA) { return 1; }
if(tMotor==motorB) { return 2; }
if(tMotor==motorC) { return 3; }
return 0;
}
//////////////////////////////////////////////////////////////////////
// M O T O R E N C O D E R
// The MotorEncoder is a variable which keeps track of the number of degrees the motor has turned.
// This value is not purged until the robot is turned off. This means that you can re-start a
// program and the MotorEncoder just keeps on counting. If the program checks for a certain value
// (turning or going straight) and the encoder value cycles back to 0, then the program may result
// in a logic error. To prevent this from happening, disallow any target values beyond the operating
// range. If this results in a problem, insert resetRotationSensor() functions within the program.
void checkForEncoderError(int newValue) {
if(newValue > 32000 || newValue < -32000) {
nxtDisplayCenteredTextLine(1, "ERROR: Over Max");
nxtDisplayCenteredTextLine(2, "for nMotorEncoder");
nxtDisplayCenteredTextLine(3, "Power down Robot.");
nxtDisplayCenteredTextLine(4, "Reset the encoder");
nxtDisplayCenteredTextLine(4, "in your program.");
waitForOrangeButton();
}
}
//////////////////////////////////////////////////////////////////////
// Variables used to Configure Robot Dimensions
// Robot dimensions are stored in a text file on the NXT brick. This allows for functions to be
// developed independent of the physical characteristics of the Robot. Thus, different robots
// could use the same code as long as each robot's physical dimensions are stored in this file.
// Define internal constants and temporary handles for the file processes.
const string sFileNameRD = "RobotDimensions.txt";
TFileIOResult nIoResult;
TFileHandle hFileHandle;
int defaultFileSize = 40; // Only need space to store 4 Float values. Each Float value uses 4 bytes.
int nFileSize = defaultFileSize; // Only need space to store 4 Float values. Each Float value uses 4 bytes.
// There are 4 characteristics of the robot which are used in various move functions.
// Each of the size characteristics are entered in milimeters (MM).
// The four lines below are used only to declare these variables as Global.
// Do not enter any values for these variables here. The values to be used by the program are read
// from the text file (mentioned earlier). The file is read in the InitMove() function below.
float centerOfWheelToCenterOfRobotMM = 0; // Distance of each drive wheel to the center of the robot
// Be careful with tankbots. This distance is to the sprockets, not the center of the tread.
float wheelDiameterMM = 43.2; // Self explanatory. Just look at the tire to see the diameter in MM. Use a default of 43.2--that's the typical tire we use
float wheelCircumferenceMM = 0; // The computer will figure this one.
float gearRatio = 1; // Used as a multiplier to figure out the number of degrees of rotation.
//////////////////////////////////////////////////////////////////////
// Although not absolutely necessary to the operation of a robot, a "configuration" file is stored on the robot which
// contains the physical dimensions used to calculate values to be stored in variables. These variables help the robot
// to perform consistently across different environments and physical dimensions. This file will be created if it does
// not already exist on the robot.
//
void createRobotDimensionsConfig(int MessageType) {
if(MessageType==1) {
int i;
display(0,"*****************");
display(2,"*Missing Config *");
display(3,"*file. I will *");
display(4,"*create default *");
display(5,"*file instead. *");
display(6,"*Press Orange Btn");
display(7,"*to continue. *");
while(nNxtButtonPressed != kEnterButton) {
for(i=1;i<80;i++) { wait10Msec(1);
if(i<40) {
display(1,"* WARNING *");
} else {
display(1,"* *");
}
if(nNxtButtonPressed == kEnterButton) break;
}
}
}
CloseAllHandles(nIoResult);
wait1Msec(500);
// You can edit the values for the next two variables.
// Please note: the values must be entered in terms of millimeters.
float centerOfWheelToCenterOfRobotMM = 45.0;
float wheelDiameterMM = 56.0;
//
// Get the Wheel's circumference
float wheelCircumference = wheelDiameterMM * PI; // Formula. NOT changeable.
//
// If no gears are used, then the gear ratio should be "(float) 1 / 1"
gearRatio = 1.0;
// gearRatio = (float) 24 / 40; (Example of two gears)
// Format for the gear ratio is:
// If 2 gears: (Gear that connects to the wheel) / (gear that connects to the motor)
// If 3 gears: a / b / c
// where a = the gear that connects to the wheel
// b = the gear in the middle
// c = the gear that connects to the motor
// In general, start with the gear at the wheel and divide it by each gear
// in sequential order as you get closer to the gear connected to the motor
// Do not change the lines below
Delete(sFileNameRD, nIoResult);
hFileHandle = 0; nFileSize = defaultFileSize;
OpenWrite( hFileHandle, nIoResult, sFileNameRD, nFileSize);
WriteFloat(hFileHandle, nIoResult, centerOfWheelToCenterOfRobotMM);
WriteFloat(hFileHandle, nIoResult, wheelDiameterMM);
WriteFloat(hFileHandle, nIoResult, wheelCircumference);
WriteFloat(hFileHandle, nIoResult, gearRatio);
Close(hFileHandle, nIoResult);
PlaySoundFile("OK.rso");
}
//////////////////////////////////////////////////////////////////////
//
// Each robot may be constructed differently (wheel diameter, dimensions, ...)
// In order for the same functions to result in the same performance on every robot, it is necessary to
// obtain the physical dimensions of the robot. These dimensions are stored in a variables used in this file.
// The variables are defined and then initialized through one of the two functions below.
// A default set of values is stored in a text file located on the robot itself. The purpose of storing them
// in a text file is that it is a more permanent storage. This same file is read by all programs since each program
// must have the InitMove() function. The text file is created using the function: createRobotDimensionsConfig().
// If this file does not exist on the robot (as when you download the firmware), the robot will create one upon initialization.
//
void InitMove() {
CloseAllHandles(nIoResult);
hFileHandle = 0; nFileSize = defaultFileSize;
OpenRead( hFileHandle, nIoResult, sFileNameRD, nFileSize);
if(nIoResult==ioRsltFileNotFound) {
createRobotDimensionsConfig(1); // Create the file and try again
OpenRead( hFileHandle, nIoResult, sFileNameRD, nFileSize);
}
ReadFloat(hFileHandle, nIoResult, centerOfWheelToCenterOfRobotMM);
ReadFloat(hFileHandle, nIoResult, wheelDiameterMM);
ReadFloat(hFileHandle, nIoResult, wheelCircumferenceMM);
ReadFloat(hFileHandle, nIoResult, gearRatio);
Close(hFileHandle, nIoResult);
// Also setup a few sensors
SensorType[touchSensor] = sensorTouch; // Set 'touchSensor' to be of type sensorTouch
SensorType[sonarSensor] = sensorSONAR; // Set 'sonarSensor' to be of type sensorSONAR
SensorType[lightSensorA] = sensorLightInactive;
SensorType[lightSensorB] = sensorLightInactive;
SensorType[colorSensor] = sensorLightInactive;
}
// The configureRobotDimensions() function is an overloaded function.
void configureRobotDimensions(float WheelToCenterOfRobot, float WheelDiameter, float DriveGearRatio, int RobotForwardIs, int AccessoryForwardIs ) {
centerOfWheelToCenterOfRobotMM = WheelToCenterOfRobot;
wheelDiameterMM = WheelDiameter;
wheelCircumferenceMM = wheelDiameterMM * PI; // Formula. NOT changeable.
gearRatio = (float) DriveGearRatio;
mDir=RobotForwardIs;
aDir=AccessoryForwardIs;
mMotor = (mDir == FORWARD) ? motorB : motorC;
sMotor = (mDir == FORWARD) ? motorC : motorB;
}
// The configureWheelToCenterOfRobotMM() function is generally not used since the configureRobotDimensions() function performs most work
void configureWheelToCenterOfRobotMM(float WheelToCenterOfRobot) {
centerOfWheelToCenterOfRobotMM = WheelToCenterOfRobot;
}
void configureWheelDiameterMM(float WheelDiameter) {
wheelDiameterMM = WheelDiameter;
wheelCircumferenceMM = wheelDiameterMM * PI; // Formula. NOT changeable.
}
// Overload the Configuration function for default motor orientation
void configureRobotDimensions(float WheelToCenterOfRobot, float WheelDiameter, float DriveGearRatio, int RobotForwardIs) {
configureRobotDimensions(WheelToCenterOfRobot, WheelDiameter, DriveGearRatio, RobotForwardIs, FORWARD);
}
// Overload the Configuration function for default motor orientation
void configureRobotDimensions(float WheelToCenterOfRobot, float WheelDiameter, float DriveGearRatio) {
configureRobotDimensions(WheelToCenterOfRobot, WheelDiameter, DriveGearRatio, FORWARD, FORWARD);
}
// Overload function for default gear ratio and motor orientation
void configureRobotDimensions(float WheelToCenterOfRobot, float WheelDiameter) {
configureRobotDimensions(WheelToCenterOfRobot, WheelDiameter, 1.0, FORWARD, FORWARD);
}
////////////////////////////////////////////////////////////////////////////////////
//
// Define the variables and constants to be used with LineFollowing and working with lines.
//
// This robot will have two light sensors. Each sensor has a slightly different range.
// Some of these differences can be as much as 10%. To account for the differences, a separate
// set of ranges is created for each light sensor.
// A string constant is created to store the filename for the file that will contain the range information
const string sFileNameLR = "LightRange.txt";
// A Low and High light value is created for each light sensor. This library is setup to accommodate two light sensors.
// However, if only one light sensor is used, then that's OK too.
long tLowLight = 300; // Some default values are shown here just in case the user never actually calibrates
long tHighLight = 600; // the light sensors. In that case, the default values will allow the program and robot
long tLowLight2 = 300; // to function at a basic level. But calibration would result in optimal performance.
long tHighLight2 = 600;
//
bool IsFollowingLine = false; // Some of the Stop functions will need to know if the robot is following a line or not.
// If the robot is following a line, there is extra code in the While() loop in the
// stopOnTouch(), stopAfterDistance(), and stopIfCloseTo()
//
// Declare Internal Methods to be performed for working with lines.
//
// These two functions are generally not referenced by the programmer.
// They are for internal purposes in dealing with the output of the light sensors
float getLightPortion() {
float result;
long numerator, denominator;
// In this particular function, the robot is assumed to have two light sensors where LightSensorA is in the
// front of the robot and LightSensorB is in the back of the robot.
if(direction == BACKWARD) {
numerator = (long) SensorRaw[lightSensorB] - tLowLight2;
denominator = tHighLight2 - tLowLight2;
} else {
numerator = (long) SensorRaw[lightSensorA] - tLowLight;
denominator = tHighLight - tLowLight;
}
result = (float) numerator / denominator;
if(result < 0) { result=0; }
if(result > 1) { result=1; }
return result;
}
float getLightPortionA() { //This function only looks at the portion of light at LightSensorA
float result;
long numerator, denominator;
// Reset the light range if necessary. Allows the robot to dynamically re-calibrate itself as needed.
numerator = (long) SensorRaw[lightSensorA] - tLowLight;
denominator = tHighLight - tLowLight;
result = (float) numerator / denominator;
if(result < 0) { result=0; }
if(result > 1) { result=1; }
return result;
}
float getLightPortionB() { //This function only looks at the portion of light at LightSensorB
float result;
long numerator, denominator;
// Reset the light range if necessary. Allows the robot to dynamically re-calibrate itself as needed.
numerator = (long) SensorRaw[lightSensorB] - tLowLight2;
denominator = tHighLight2 - tLowLight2;
result = (float) numerator / denominator;
if(result < 0) { result=0; }
if(result > 1) { result=1; }
return result;
}
// Create a few basic functions that perform some simple calculations
// It may be easier to use percentages (0 to 100) in some parts of the program than to use portions (0 to 1)
float getLightPercent() { return 100 * getLightPortion(); }
float getLightPercentA() { return 100 * getLightPortionA(); }
float getLightPercentB() { return 100 * getLightPortionB(); }
// This function will return a value from +1 to -1 depending on the comparison of the light sensors.
// If both light sensors show the same value, then the function returns '0'. If one light sensor is on a line and the other
// on a white space, then the value returned is either +1 or -1 depending on which light sensor is on the line.
float getLightDiffPortion() {
float result;
long numerator1;
long numerator2;
long numerator;
numerator1 = (float) (SensorRaw[lightSensorB] - tLowLight) / (tHighLight - tLowLight) * 100;
numerator2 = (float) (SensorRaw[lightSensorA] - tLowLight2) / (tHighLight2 - tLowLight2) * 100;
numerator = (float) numerator1 - numerator2 + 100;
result = (float) numerator / 200;
return result;
}
//////////////////////////////////////////////////////////////////////////////////////////
// Calibrating the light sensor is a lot of fun and ensures that the program is using optimal values for detecting lines.
// This function will ask the user to pick up and move the robot over the line(s) and will get a variety of light sensor readings.
// The values of the light sensor readings (high and low) for both sensors are stored in a text file on the robot.
// The robot then reads this text file each time a program is run. The text file enables a more permanent storage of the
// light sensor values and eliminates the need from having to re-calibrate the robot each time a program is run.
// The light sensors only need calibrated once. A variable is passed to this function called: MessageType
// This variable is used to simply show a slightly different text message on the display.
void createLightCalibrationFile(int MessageType) {
int i;
if(MessageType==0) { // This is the typical message if the function is called from within the program
wait1Msec(500);
display(0,"*****************");
display(1,"* Light Sensor *");
display(2,"* Calibration *");
display(3,"* *");
display(4,"* Take Robot to *");
display(5,"* the line. *");
waitForOrangeButton();
}
if(MessageType==1) { // This is a message shown if the robot is missing the configuration file which could happen
// if the firmware is re-downloaded
wait1Msec(500);
display(0,"*****************");
display(2,"*Light Sensors *");
display(3,"*not calibrated *");
display(4,"*Take the robot *");
display(5,"*to the line. *");
display(6,"*Press Orange Btn");
display(7,"*to continue. *");
while(nNxtButtonPressed != kEnterButton) {
for(i=1;i<80;i++) { wait10Msec(1);
if(i<40) {
display(1,"* WARNING *");
} else {
display(1,"* *");
}
if(nNxtButtonPressed == kEnterButton) break;
}
}
}
////////////////////////////////////////////////////////////////////////////////////
//
// Declare several initial values to be used for comparisons. Although it may seem counter intuitive that the lowLight variables
// would have such high initial values and the highLight variables would have such low initial values, the expectation is that
// the observed light value will deviate from the initial state of the variable and thus cause a new value to be written to
// the variable. The lowest observed light value will be stored in the variable lowLight. If the variable was initialized to
// 0 then there would be no light value that would be lower. We set the initial value to 9999 because we would expect that an
// observed light value will be less than that.
//
long tLowLight = 9999;
long tHighLight = 0;
long tCurrLight = 0;
long tLowLight2 = 9999;
long tHighLight2 = 0;
long tCurrLight2 = 0;
CloseAllHandles(nIoResult);
wait1Msec(500);
PlaySoundFile("OK.rso");
SensorType[S3] = sensorLightActive; // Turn light 'on' if it is currently 'off'
SensorType[S2] = sensorLightActive; // Turn light 'on' if it is currently 'off'
wait1Msec(500);
nxtDisplayCenteredTextLine(1, "Move over line");
nxtDisplayCenteredTextLine(2, "back and forth");
nxtDisplayCenteredTextLine(3, "several times.");
nxtDisplayCenteredTextLine(4, "Press orange");
nxtDisplayCenteredTextLine(5, "when done.");
wait1Msec(2000);
while(nNxtButtonPressed != kEnterButton) {
wait1Msec(500);
tCurrLight= SensorRaw[lightSensorA];
if(tCurrLight < tLowLight) { tLowLight = tCurrLight; }
if(tCurrLight > tHighLight) { tHighLight = tCurrLight; }
tCurrLight2= SensorRaw[lightSensorB];
if(tCurrLight2 < tLowLight2) { tLowLight2 = tCurrLight2; }
if(tCurrLight2 > tHighLight2) { tHighLight2 = tCurrLight2; }
nxtDisplayCenteredTextLine(6, "%d to %d", tLowLight, tHighLight);
nxtDisplayCenteredTextLine(7, "%d to %d", tLowLight2, tHighLight2);
}
Delete(sFileNameLR, nIoResult); // Get rid of the file in case it already exists
hFileHandle = 0; nFileSize = defaultFileSize;
OpenWrite(hFileHandle, nIoResult, sFileNameLR, nFileSize); // Create a new file with the new values
WriteLong(hFileHandle, nIoResult, tLowLight);
WriteLong(hFileHandle, nIoResult, tHighLight);
WriteLong(hFileHandle, nIoResult, tLowLight2);
WriteLong(hFileHandle, nIoResult, tHighLight2);
Close(hFileHandle, nIoResult); // Close the file. Don't leave file handles open if not being used
tHighLight = 0;
tCurrLight = 0;
nFileSize = defaultFileSize;
OpenRead(hFileHandle, nIoResult, sFileNameLR, nFileSize); // Now try reading the file you just created
ReadLong(hFileHandle, nIoResult, tLowLight);
ReadLong(hFileHandle, nIoResult, tHighLight);
ReadLong(hFileHandle, nIoResult, tLowLight2);
ReadLong(hFileHandle, nIoResult, tHighLight2);
Close(hFileHandle, nIoResult);
display(0,"*****************");
display(1,"* *");
display(2,"* ALL DONE *");
display(3,"* *");
display(4,"*Robot is Ready *");
display(5,"* *");
display(6,"*Press Orange Btn");
display(7,"* to continue. *");
PlaySoundFile("OK.rso");
while(nNxtButtonPressed != kEnterButton) {
wait1Msec(500);
}
SensorType[S2] = sensorLightInactive; // Go ahead and turn the light 'off'
SensorType[S3] = sensorLightInactive; // Go ahead and turn the light 'off'
}
// This function retrieves the light range data for the two light sensors
void initLightSensor() {
CloseAllHandles(nIoResult);
hFileHandle = 0; nFileSize = defaultFileSize;
OpenRead(hFileHandle, nIoResult, sFileNameLR, nFileSize);
if(nIoResult==ioRsltFileNotFound) {
createLightCalibrationFile(1); // Create the file and try again
OpenRead( hFileHandle, nIoResult, sFileNameLR, nFileSize);
}
ReadLong(hFileHandle, nIoResult, tLowLight); ReadLong(hFileHandle, nIoResult, tHighLight);
ReadLong(hFileHandle, nIoResult, tLowLight2); ReadLong(hFileHandle, nIoResult, tHighLight2);
Close(hFileHandle, nIoResult);
}
void calibrateLightSensor() { createLightCalibrationFile(0); }
//////////////////////////////////////////////////////////////////////
//
// Various LineFollowing Functions
//
// This is the PID controller part of the line following function.
// This part of the function is referenced in the While() loops contained in the various Stop functions.
// The basic process is to initialize the LineFollowing function (above) and then let the other functions
// reference the PID controller (below).
void FollowLinePID() {
float dFactor = .75; // Used as a multiplier to change the power to the motors from + to - if going in reverse.
if(direction == BACKWARD) { dFactor = -0.75;}// If not going forward, then you must be in reverse.
float variablePower = powerLevel * 0.75 * gearRatio; // variablePower should be a portion of the default powerLevel.
float Kp = 0; // Using the "PID minus the ID" formula
Kp = ((getLightPercent() - (float) 50) / (float) 50 ) * variablePower; // Calculates Kp as a percentage of the variablePower amount.
nxtDisplayTextLine(4, "Lght: %7.3f", getLightPercent()); // Just a debug message
if(sideOfLine * mDir == RIGHTSIDE) { Kp *= -1; } // Switch the sign of Kp if you want to follow the other side of the line.
motor[mMotor] = (int) (dFactor * (float) ((float) powerLevel + Kp) * mDir); // Add Kp to motor1 whether going forward or backward
motor[sMotor] = (int) (dFactor * (float) ((float) powerLevel - Kp) * mDir); // Subtract Kp from motor2 whether going forward or backward
nxtDisplayTextLine(5, "Pwr %4d %4d", motor[sMotor], motor[mMotor]); // Just a debug message
//
// WAIT a few milliseconds.
wait1Msec(40); // Without this delay, the one motor seemed perform erratically (speeding up and slowing down).
// I think this may have been caused by the rapid changes to the power levels.
// By introducing the wait here, the changes to power levels occur less frequently.
// I could tell it wasn't just the motor itself because the motors performed fine when a constant power was given.
// I used the display values so I could monitor what numbers were given to the motors.
// If the line following does not appear smooth, you may want to adjust this value.
}
//////////////////////////////////////////////////////////////////////
//
// Functions to get the robot moving
//
void setPowerLevel(int power) {
powerLevel = power; // Set the global variable to the value that was passed
powerLevelRev = -power; // Set the reverse power variable to the negative of the value that was passed
}
void setAccessoryPowerLevel(int power) {
powerLevelAccessory = power; // Set the global variable for the Accessory Power Level
}
void setDefaultSpeed(int power) { setPowerLevel(power); }
void beginDrivingForward(int tempPower) {
if(lastDirection == BACKWARD) { addToDistance = true; resetRotationSensor(motorB, motorC); }
direction = FORWARD; // Set the direction so other functions know which way we are going
lastDirection= direction;
IsFollowingLine = false; // Just driving forward is not following any line
synchMotors(mMotor); // Only the master motor needs power since the other moter is synched to the master
motor[mMotor] = direction * tempPower * mDir;
}
void beginDrivingBackward(int tempPower) {
nSyncedMotors = synchNone; // De-sync the motors. The slave motor encoder cannot be altered while synced.
if(lastDirection == FORWARD) { addToDistance = true; resetRotationSensor(motorB, motorC); }
direction = BACKWARD; // Set the direction so other functions know which way we are going
lastDirection= direction;
IsFollowingLine = false; // Just driving backward is not following any lines
synchMotors(mMotor); // Only the master motor needs power since the other moter is synched to the master
motor[mMotor] = direction * tempPower * mDir;
nxtDisplayTextLine(4, "PwrLvl: %d %d", tempPower, motor[mMotor]); // Just a debug message
}
void beginFollowingWall(int thisSide, int tempPower) {
if(direction == STOPPED) beginDrivingForward(tempPower);
IsFollowingLine = false; // Just driving forward is not following any line
synchMotors(mMotor); // Only the master motor needs power since the other moter is synched to the master
nSyncedTurnRatio = 97; // One Motor will be 90% the value of the other
motor[mMotor] = direction * tempPower * mDir;
}
//////////////////////////////////////////////////////////////////////
//
// Various Turning Functions
//
// There are three types of robot turns:
// 1) Point Turn - turn in place. Wheels move in opposite directions
// 2) Swing Turn - A small radius turn where only one wheel moves. Other is stopped.
// 3) Gradual Turn - A large radius turn with both wheels moving--one more than the other.
//
// Theoretically it is possible to have just one turning function (i.e., Gradual Turn) where
// the turning radius would be zero for a point turn and would be equal to the distance between the wheels for a Swing Turn.
//
// There are two directions the robot can turn: clockwise and counterclockwise
// I wanted to avoid saying turn to the right or left because it is not clear what the subject of the turn is if
// you just say "turn to the right". Is that 'your' right or the 'robots' right?
// Turning clockwise and counterclockwise seems more definite.
//
void pointTurnClockwise(float turnDegrees, int tempPower) {
float turningCircumferenceMM = (2 * centerOfWheelToCenterOfRobotMM * PI);
float wheelRotationsPerRobotTurn = turningCircumferenceMM / wheelCircumferenceMM;
float wheelDegreesOfTurningNeeded = wheelRotationsPerRobotTurn * turnDegrees;
float targetDegrees = wheelDegreesOfTurningNeeded * gearRatio;
display(6," PointTurn *"); // Clear the message
display(7,"* Clockwise *");
targetDegrees = targetDegrees + nMotorEncoder[mMotor];
checkForEncoderError(targetDegrees); // Setting a MotorEncoder to an invalid number may result in an error. Better check first.
synchMotors(mMotor);
if(keepGoing) {
bFloatDuringInactiveMotorPWM = true; // coast or float
} else {
bFloatDuringInactiveMotorPWM = false;// brake
}
nSyncedTurnRatio = -100; // motors move in opposite directions of one another
motor[mMotor] = tempPower; // turns the motor on at specified power
while(nMotorEncoder[mMotor] <= targetDegrees) { }
if(!keepGoing) motor[mMotor] = 0; // turn the motor off.
bFloatDuringInactiveMotorPWM = false; // brake
IsFollowingLine = false; // Turning means you are not following any line
addToDistance = false;
}
void swingTurnClockwise(float turnDegrees, int tempPower) {
float turningCircumferenceMM = (4 * centerOfWheelToCenterOfRobotMM * PI);
float wheelRotationsPerRobotTurn = turningCircumferenceMM / wheelCircumferenceMM;
float wheelDegreesOfTurningNeeded = wheelRotationsPerRobotTurn * turnDegrees;
float targetDegrees = wheelDegreesOfTurningNeeded * gearRatio;
display(3,"* SwingTurn *"); // Clear the message
display(4,"* Clockwise *");
if(direction == STOPPED) beginDrivingForward(powerLevel);
nSyncedMotors = synchNone; // de-synch the motors
if(keepGoing) {
bFloatDuringInactiveMotorPWM = true; // coast or float
} else {
bFloatDuringInactiveMotorPWM = false;// brake
}
if(mDir == FORWARD) {
targetDegrees = nMotorEncoder[mMotor] + targetDegrees;
checkForEncoderError(targetDegrees); // Setting a MotorEncoder target to an invalid number may result in a logic error. Better check first.
motor[mMotor] = tempPower; // turns the motor on at specified power
while(nMotorEncoder[mMotor] <= targetDegrees) {
nxtDisplayTextLine(5, "Target: %d", targetDegrees); // Just a debug message
nxtDisplayTextLine(6, "Curr Deg: %d", nMotorEncoder[mMotor]); // Just a debug message
}
if(!keepGoing) motor[mMotor] = 0; // turn the motor off.
} else {
targetDegrees = nMotorEncoder[sMotor] - targetDegrees;
checkForEncoderError(targetDegrees); // Setting a MotorEncoder target to an invalid number may result in a logic error. Better check first.
motor[sMotor] = tempPower * -1; // turns the motor on at specified power
while(nMotorEncoder[sMotor] >= targetDegrees) {
nxtDisplayTextLine(5, "Target: %d", targetDegrees); // Just a debug message
nxtDisplayTextLine(6, "Curr Deg: %d", nMotorEncoder[sMotor]); // Just a debug message
}
if(!keepGoing) motor[sMotor] = 0; // turn the motor off.
}
bFloatDuringInactiveMotorPWM = false; // brake
IsFollowingLine = false; // Turning means you are not following any line
addToDistance = false;
}
void gradualTurnClockwise(float turnDegrees, float turnRadiusIN, int tempPower) {
float innerTurnRadius = (turnRadiusIN * 25.4) - centerOfWheelToCenterOfRobotMM;
// Convert inches to millimeters by multiplying by 25.4
if(innerTurnRadius < 0) { innerTurnRadius = 0; }
float outerTurnRadius = (turnRadiusIN * 25.4) + centerOfWheelToCenterOfRobotMM;
float outerTurningCircumferenceMM = (2 * outerTurnRadius * PI);
float wheelRotationsPerRobotTurn = outerTurningCircumferenceMM / wheelCircumferenceMM;
float wheelDegreesOfTurningNeeded = wheelRotationsPerRobotTurn * turnDegrees;
float targetDegrees = wheelDegreesOfTurningNeeded * gearRatio;
float turnRatio;
if(keepGoing) {
bFloatDuringInactiveMotorPWM = true; // coast or float
} else {
bFloatDuringInactiveMotorPWM = false; // brake
}
display(6," GradualTurn *"); // Clear the message
display(7,"* Clockwise *");
turnRatio = innerTurnRadius / outerTurnRadius * 100.0;
nxtDisplayCenteredTextLine(3, "TurnRatio %6.3f", turnRatio); // Just a debug message
// The turn ratio represents a percentage that the slave motor will turn in relation to the master
// Because we're turning clockwise, B is the master and the turn ratio is the ratio of inner/outer.
if(direction == STOPPED) beginDrivingForward(tempPower);
if(direction * mDir == FORWARD) {
targetDegrees = nMotorEncoder[mMotor] + targetDegrees;
checkForEncoderError(targetDegrees); // Setting a MotorEncoder target to an invalid number may result in a logic error. Better check first.
synchMotors(mMotor);
nSyncedTurnRatio = turnRatio; // turn ratio is set after the motors are synced
motor[mMotor] = tempPower; // turns the motor on at specified power
while(nMotorEncoder[mMotor] <= targetDegrees) { } //continue until the Target position is reached
if(!keepGoing) motor[mMotor] = 0; // turn the motor off.
} else {
targetDegrees = nMotorEncoder[sMotor] - targetDegrees;
checkForEncoderError(targetDegrees); // Setting a MotorEncoder target to an invalid number may result in a logic error. Better check first.
synchMotors(sMotor);
nSyncedTurnRatio = turnRatio;
motor[sMotor] = -1 * tempPower; // turns the motor on at specified power
while(nMotorEncoder[sMotor] >= targetDegrees) { } //continue until the Target position is reached
if(!keepGoing) motor[sMotor] = 0; // turn the motor off.
}
bFloatDuringInactiveMotorPWM = false; // brake
IsFollowingLine = false; // Turning means you are not following any line
addToDistance = false;
}
void pointTurnCounterClockwise(float turnDegrees, int tempPower) {
float turningCircumferenceMM = (2 * centerOfWheelToCenterOfRobotMM * PI);
float wheelRotationsPerRobotTurn = turningCircumferenceMM / wheelCircumferenceMM;
float wheelDegreesOfTurningNeeded = wheelRotationsPerRobotTurn * turnDegrees;
float targetDegrees = wheelDegreesOfTurningNeeded * gearRatio;
display(6," PointTurn *"); // Clear the message
display(7,"*CounterClockwise");
targetDegrees = targetDegrees + nMotorEncoder[sMotor];
checkForEncoderError(targetDegrees); // Setting a MotorEncoder target to an invalid number may result in a logic error. Better check first.
synchMotors(sMotor);
if(keepGoing) {
bFloatDuringInactiveMotorPWM = true; // coast or float
} else {
bFloatDuringInactiveMotorPWM = false;// brake
}
nSyncedTurnRatio = -100; //motors move in opposite directions of one another
motor[sMotor] = tempPower; //turns the motor on at specified power
while(nMotorEncoder[sMotor] <= targetDegrees) { }
//continue to power motorC until the motor nMotorEncoderTarget position is reached
if(!keepGoing) motor[sMotor] = 0; // turn the motor off.
bFloatDuringInactiveMotorPWM = false; // brake
IsFollowingLine = false; // Turning means you are not following any line
addToDistance = false;
}
void swingTurnCounterClockwise(float turnDegrees, int tempPower) {
float turningCircumferenceMM = (4 * centerOfWheelToCenterOfRobotMM * PI);
float wheelRotationsPerRobotTurn = turningCircumferenceMM / wheelCircumferenceMM;
float wheelDegreesOfTurningNeeded = wheelRotationsPerRobotTurn * turnDegrees;
float targetDegrees = wheelDegreesOfTurningNeeded * gearRatio;
display(3,"* SwingTurn *"); // Clear the message
display(4,"*CounterClockwise");
if(direction == STOPPED) beginDrivingForward(powerLevel);
nSyncedMotors = synchNone; // de-synch the motors
if(keepGoing) {
bFloatDuringInactiveMotorPWM = true; // coast or float
} else {
bFloatDuringInactiveMotorPWM = false;// brake
}
if(mDir == FORWARD) {
targetDegrees = nMotorEncoder[sMotor] + targetDegrees;
checkForEncoderError(targetDegrees); // Setting a MotorEncoder target to an invalid number may result in a logic error. Better check first.
motor[sMotor] = tempPower; // turns the motor on at specified power
while(nMotorEncoder[sMotor] <= targetDegrees) {
nxtDisplayTextLine(5, "Target: %d", targetDegrees); // Just a debug message
nxtDisplayTextLine(6, "Curr Deg: %d", nMotorEncoder[sMotor]); // Just a debug message
}
if(!keepGoing) motor[sMotor] = 0; // turn the motor off.
} else {
targetDegrees = nMotorEncoder[mMotor] - targetDegrees;
checkForEncoderError(targetDegrees); // Setting a MotorEncoder target to an invalid number may result in a logic error. Better check first.
motor[mMotor] = tempPower * -1; // turns the motor on at specified power
while(nMotorEncoder[mMotor] >= targetDegrees) {
nxtDisplayTextLine(5, "Target: %d", targetDegrees); // Just a debug message
nxtDisplayTextLine(6, "Curr Deg: %d", nMotorEncoder[mMotor]); // Just a debug message
}
if(!keepGoing) motor[mMotor] = 0; // turn the motor off.
}
bFloatDuringInactiveMotorPWM = false; // brake
IsFollowingLine = false; // Turning means you are not following any line
addToDistance = false;
}
void gradualTurnCounterClockwise(float turnDegrees, float turnRadiusIN, int tempPower) {
float innerTurnRadius = (turnRadiusIN * 25.4) - centerOfWheelToCenterOfRobotMM;
// Convert inches to millimeters by multiplying by 25.4
if(innerTurnRadius < 0) { innerTurnRadius = 0; }
float outerTurnRadius = (turnRadiusIN * 25.4) + centerOfWheelToCenterOfRobotMM;
float outerTurningCircumferenceMM = (2 * outerTurnRadius * PI);
float wheelRotationsPerRobotTurn = outerTurningCircumferenceMM / wheelCircumferenceMM;
float wheelDegreesOfTurningNeeded = wheelRotationsPerRobotTurn * turnDegrees;
float targetDegrees = wheelDegreesOfTurningNeeded * gearRatio;
float turnRatio;
if(keepGoing) {
bFloatDuringInactiveMotorPWM = true; // coast or float
} else {
bFloatDuringInactiveMotorPWM = false; // brake
}
display(6," GradualTurn *"); // Clear the message
display(7,"*CounterClockwise");
turnRatio = innerTurnRadius / outerTurnRadius * 100.0;
// The turn ratio represents a percentage that the slave motor will turn in relation to the master
// Because we're turning clockwise, B is the master and the turn ratio is the ratio of inner/outer.
if(direction == STOPPED) beginDrivingForward(tempPower);
if(direction * mDir == FORWARD) {
targetDegrees = nMotorEncoder[sMotor] + targetDegrees;
checkForEncoderError(targetDegrees); // Setting a MotorEncoder target to an invalid number may result in a logic error. Better check first.
synchMotors(sMotor);
nSyncedTurnRatio = turnRatio; // turn ratio is set after the motors are synced
motor[sMotor] = tempPower; // turns the motor on at specified power
while(nMotorEncoder[sMotor] <= targetDegrees) { } // until the Target position is reached
if(!keepGoing) motor[sMotor] = 0; // turn the motor off.
} else {
targetDegrees = nMotorEncoder[mMotor] - targetDegrees;
checkForEncoderError(targetDegrees); // Setting a MotorEncoder target to an invalid number may result in a logic error. Better check first.
synchMotors(mMotor);
nSyncedTurnRatio = turnRatio; // turn ratio is set after the motors are synced
motor[mMotor] = -1 * tempPower; // turns the motor on at specified power
while(nMotorEncoder[mMotor] >= targetDegrees) { } // until the motor Target position is reached
if(!keepGoing) motor[mMotor] = 0; // turn the motor off.
}
bFloatDuringInactiveMotorPWM = false; // brake
IsFollowingLine = false; // Turning means you are not following any line
addToDistance = false;
}
void Motor(int thisMotor, float turnDegrees, int dirRotation) {
nSyncedMotors = synchNone; // No motor synch
bFloatDuringInactiveMotorPWM = false; // Enable braking
float targetDegrees;
if(turnDegrees < 0) { // Turning a negative number of degrees?
dirRotation *= -1; // Reverse the direction
turnDegrees = abs(turnDegrees); // Make sure you are turning a positive number of degrees in the new direction
}
dirRotation *= mDir;
//float initialMotorEncoderValue = nMotorEncoder[thisMotor];
if(dirRotation == BACKWARD) {
targetDegrees = ((nMotorEncoder[thisMotor] - turnDegrees) - motorError[convertMotorToInt(thisMotor)]);
checkForEncoderError(targetDegrees); // Setting a MotorEncoder target to an invalid number may result in a logic error. Better check first.
if(thisMotor == (int) motorA) {
motor[thisMotor] = powerLevelAccessory * -1;
} else {
motor[thisMotor] = powerLevelRev; // *mDir;//turns the motor on at specified power
}
nMotorEncoderTarget[thisMotor] = targetDegrees; //powerLevelrget
while(nMotorEncoder[thisMotor] >= targetDegrees - ((float)powerLevelRev * ((float) abs(powerLevelRev)/150.0))) { }
} else {
targetDegrees = ((turnDegrees + nMotorEncoder[thisMotor]) - motorError[convertMotorToInt(thisMotor)]);
checkForEncoderError(targetDegrees); // Setting a MotorEncoder target to an invalid number may result in a logic error. Better check first.
if(thisMotor == (int) motorA) {
motor[thisMotor] = powerLevelAccessory;
} else {
motor[thisMotor] = powerLevel; // * mDir; //turns the motor on at specified power
}
nMotorEncoderTarget[thisMotor] = targetDegrees; // sets a target
while(nMotorEncoder[thisMotor] <= targetDegrees - ((float)powerLevel * ((float) abs(powerLevel)/150.0))) { }
}
motor[thisMotor] = 0; // turns the motor on at specified power
wait10Msec(10); // Pause a little to make sure motor has stopped.
bFloatDuringInactiveMotorPWM = true; // Enable coast or float in case the attachment is stuck or needs to rest
motor[thisMotor] = 0;
motorError[convertMotorToInt(thisMotor)] = nMotorEncoder[convertMotorToInt(thisMotor)] - targetDegrees;
wait10Msec(2);
}
// FollowLine is a User accessed function for initializing the robot to follow a line.
// This function has a few parameters that are set when you begin to follow the line; a type of initialization process.
// Once initialized, the subsequent checks are not needed. The "IsFollowingLine" helps to coordinate the different types
// of moving with the different forms of the Stop function. In this way, each Stop function is able to determine which
// type of moving needs to take place.
void followLine(int thisSide) {
sideOfLine = thisSide; // Store which side of the line has been selected. This will
// be needed in the Stop functions that call FollowLine()
if(direction == STOPPED) { direction = FORWARD; } // If you were stopped, then let's go forward.
if(direction == FORWARD) { // Once you are already following the line, then you don't have to check this again
if(SensorType[S3] == sensorLightInactive) { SensorType[S3] = sensorLightActive; } // Make sure you turn the light 'on'
if(SensorType[S2] == sensorLightActive) { SensorType[S2] = sensorLightInactive; } // Turn it off
} else {
if(SensorType[S2] == sensorLightInactive) { SensorType[S2] = sensorLightActive; } // Make sure you turn the light 'on'
SensorType[S3] = sensorLightInactive;
}
if(getLightPercent() > 50) { // Ooops. May be off of the line
if(thisSide == RIGHTSIDE) { // sweep clockwise
pointTurnClockwise(45,30);
while(getLightPercent() > 60) {
pointTurnCounterClockwise(5,30);
}
} else { // sweep counter clockwise
pointTurnCounterClockwise(45,30);
while(getLightPercent() > 60) {
pointTurnClockwise(5,30);
}
}
}
IsFollowingLine = true; // This variable is used for the While() loops in the Stop functions.
nSyncedMotors = synchNone; // You must De-sync the motors.
if(direction == STOPPED) { direction = FORWARD * mDir; } // If you were stopped, then let's go forward.
FollowLinePID();
}
//////////////////////////////////////////////////////////////////////
//
// Various Stop Functions
//
void powerZero() {
motor[motorA] = 0; // turn the motor off.
motor[motorB] = 0; // turn the motor off.
motor[motorC] = 0; // turn the motor off.
}
void stopMoving() {
if(keepGoing) return;
bFloatDuringInactiveMotorPWM = false; // brake
nSyncedMotors = synchNone; // De-sync the motors.
powerZero();
direction = STOPPED;
IsFollowingLine = false; // Stopping means you are not following any line
if(SensorType[S2] == sensorLightActive) { SensorType[S2] = sensorLightInactive; } // Turn it off
if(SensorType[S3] == sensorLightActive) { SensorType[S3] = sensorLightInactive; } // Turn it off
}
void untilLine() {
if(direction == STOPPED) beginDrivingForward(powerLevel);
if(direction == FORWARD) {
SensorType[S3] = sensorLightActive; // Make sure you turn the light 'on'
} else {
SensorType[S2] = sensorLightActive; // Make sure you turn the light 'on'
}
// Enter a loop and stay there as long as the light value is greater than % in variable lineIsAtLevel
display(6," Until Line *"); // Clear the message
display(7," *");
while(getLightPercent() > lineIsAtLevel) { }
stopMoving(); // Stop moving now
if(SensorType[S3]==sensorLightActive) { SensorType[S3] = sensorLightInactive; } // Go ahead and turn the light 'off'
if(SensorType[S2]==sensorLightActive) { SensorType[S2] = sensorLightInactive; } // Go ahead and turn the light 'off'
addToDistance = false;
}
void stopOnLine() { untilLine(); stopMoving(); }
bool robotSees(int seesColor) {
SensorType[colorSensor] = sensorCOLORFULL;
wait(1);
if(SensorValue[colorSensor] != seesColor) {
SensorType[colorSensor] = sensorCOLORNONE;
return false;
} else {
SensorType[colorSensor] = sensorCOLORNONE;
return true;
}
}
void untilRobotSees(int seesColor) {
SensorType[colorSensor] = sensorCOLORFULL;
int oldDirection = direction;
bool oldIsFollowingLine = IsFollowingLine;
do {
while(SensorValue[colorSensor] != seesColor) {
if(direction == STOPPED) beginDrivingForward(powerLevel);
if(direction == FORWARD) { // If going forward, then check rotation sensor <= target
if(IsFollowingLine) { FollowLinePID(); }
} else { // If going backward, then check rotation sensor >= target
if(IsFollowingLine) { FollowLinePID(); }
}
}
stopMoving();
wait(1);
SensorType[colorSensor] = sensorCOLORFULL;
wait(1);
if(SensorValue[colorSensor] != seesColor) {
if(oldDirection== BACKWARD) { beginDrivingBackward(powerLevel); } else { beginDrivingForward(powerLevel); }
IsFollowingLine = oldIsFollowingLine; // Re-initialize this because the beginDriving functions will change it
if(IsFollowingLine) { followLine(sideOfLine); }
}
display(6," *"); // Clear the message
display(7,"* *");
display(8," *"); // Clear the message
nxtDisplayTextLine(6, " %d ", SensorValue[colorSensor]);
} while(SensorValue[colorSensor] != seesColor);
SensorType[colorSensor] = sensorCOLORNONE;
addToDistance = false;
}
void untilRobotSees(int seesColor, float inches) {
SensorType[colorSensor] = sensorCOLORFULL;
int oldDirection = direction;
long rotationSensor = 0;
float targetDegrees;
rotationSensor = nMotorEncoder[motorB];
// WheelCircumferenceMM is in millimeters, so, multiply inches by 25.4 to convert to mm.
targetDegrees = 360 * inches * 25.4 / wheelCircumferenceMM * gearRatio; // Num of Degrees to turn
bool oldIsFollowingLine = IsFollowingLine;
bFloatDuringInactiveMotorPWM = false; // brake
targetDegrees *= direction;
targetDegrees += rotationSensor; // Add these degrees to the current rotation sensor
nMotorEncoderTarget[motorB] = targetDegrees; // This is the new target degree to achieve
do {
while(SensorValue[colorSensor] != seesColor && nMotorRunState[motorB] != runStateIdle && nMotorRunState[motorB] != runStateHoldPosition && nMotorEncoder[motorB] <= targetDegrees) {
if(direction == STOPPED) beginDrivingForward(powerLevel);
if(direction == FORWARD) { // If going forward, then check rotation sensor <= target
if(IsFollowingLine) { FollowLinePID(); }
} else { // If going backward, then check rotation sensor >= target
if(IsFollowingLine) { FollowLinePID(); }
}
}
powerZero();
if(nMotorEncoder[motorB] > targetDegrees) { break; }
wait(1);
SensorType[colorSensor] = sensorCOLORFULL;
wait(1);
if(SensorValue[colorSensor] != seesColor) {
if(oldDirection== BACKWARD) { beginDrivingBackward(powerLevel); } else { beginDrivingForward(powerLevel); }
IsFollowingLine = oldIsFollowingLine; // Re-initialize this because the beginDriving functions will change it
if(IsFollowingLine) { followLine(sideOfLine); }
wait(1);
}
display(6," *"); // Clear the message
display(7,"* *");
display(8," *"); // Clear the message
nxtDisplayTextLine(6, " %d ", SensorValue[colorSensor]);
} while(SensorValue[colorSensor] != seesColor && nMotorRunState[motorB] != runStateIdle && nMotorRunState[motorB] != runStateHoldPosition && nMotorEncoder[motorB] <= targetDegrees);
SensorType[colorSensor] = sensorCOLORNONE;
addToDistance = false;
}
void stopIfRobotSees(int seesColor) { untilRobotSees(seesColor); stopMoving(); }
void untilSonarLessThan(float distance_in_inches) {
distance_in_inches = distance_in_inches + (float) ((float)powerLevel / 100 * 3.0); // Add distance based on power level because it will take a few milliseconds before the robot is able to respondto the next instruction
display(3," SONAR Checking *"); // Clear the message
display(4," <<<< >>>> *");
nxtDisplayTextLine(5, "Crit %d ", distance_in_inches);
nxtDisplayTextLine(6, "Curr %d ", SensorValue[sonarSensor]);
float distance_in_cm; distance_in_cm = distance_in_inches * 2.54;
while(SensorValue[(short) sonarSensor] > distance_in_cm) {
nxtDisplayTextLine(7, "Curr %d ", SensorValue[sonarSensor]);
if(direction == STOPPED) beginDrivingForward(powerLevel);
if(IsFollowingLine) { FollowLinePID(); }
}
addToDistance = false;
}
bool isSonarLessThan(float distance_in_inches) {
float distance_in_cm; distance_in_cm = distance_in_inches * 2.54;
return (SensorValue((short) sonarSensor) < distance_in_cm);
}
bool isSonarGreaterThan(float distance_in_inches) {
float distance_in_cm; distance_in_cm = distance_in_inches * 2.54;
return (SensorValue((short) sonarSensor) > distance_in_cm);
}
void stopIfSonarLessThan(float distance_in_inches) { untilSonarLessThan(distance_in_inches); stopMoving(); }
void untilSonarGreaterThan(float distance_in_inches) {
float distance_in_cm; distance_in_cm = distance_in_inches * 2.54;
while(SensorValue(sonarSensor) <= distance_in_cm) {
if(IsFollowingLine) { FollowLinePID(); } // If going forward, then check rotation sensor
}
addToDistance = false;
}
void untilDistance(float inches) {
long rotationSensor = 0;
int currPowerLevel= motor[mMotor];
float targetDegrees;
if(addToDistance) { inches+=1; addToDistance=false; }
rotationSensor = nMotorEncoder[mMotor];
nxtDisplayCenteredTextLine(5," Until Dis: %3.3f", gearRatio); // Clear the message
display(7," *");
// WheelCircumferenceMM is in millimeters, so, multiply inches by 25.4 to convert to mm.
targetDegrees = (float) 360 * (float) inches * 25.4 / (float) wheelCircumferenceMM * (float) gearRatio; // Num of Degrees to turn
bFloatDuringInactiveMotorPWM = false; // brake
targetDegrees *= direction * mDir;
targetDegrees += rotationSensor; // Add these degrees to the current rotation sensor
checkForEncoderError(targetDegrees); // Setting a MotorEncoder target to an invalid number may result in a logic error. Better check first.
nMotorEncoderTarget[mMotor] = targetDegrees; // This is the new target degree to achieve
if(direction == STOPPED) beginDrivingForward(powerLevel);
// if(direction * mDir == FORWARD) { PlaySoundFile("Forward.rso");PlaySoundFile("Turn.rso"); }
// if(direction * mDir == BACKWARD) { PlaySoundFile("Backwards.rso"); }
nxtDisplayCenteredTextLine(6, "Encoder: %d", nMotorEncoder[mMotor]);
nxtDisplayCenteredTextLine(7, " Target: %d", targetDegrees);
//while(nMotorRunState[mMotor] != runStateIdle && nMotorRunState[mMotor] != runStateHoldPosition && ((direction*mDir==FORWARD && nMotorEncoder[mMotor] <= targetDegrees) || (direction*mDir==BACKWARD && nMotorEncoder[mMotor] >= targetDegrees))) {
while(((direction*mDir==FORWARD && nMotorEncoder[mMotor] <= targetDegrees) || (direction*mDir==BACKWARD && nMotorEncoder[mMotor] >= targetDegrees))) {
motor[mMotor] = currPowerLevel;
nxtDisplayCenteredTextLine(2, "mMotorPwr: %d", motor[mMotor]);
nxtDisplayCenteredTextLine(3, "sMotorPwr: %d", motor[sMotor]);
if(IsFollowingLine) { FollowLinePID(); }
}
// PlaySoundFile("Bravo.rso");
}
void stopAfterDistance(float inches) { untilDistance(inches); stopMoving(); }
void untilRotations(float turnRotations) {
long rotationSensor = 0;
float targetDegrees;
rotationSensor = nMotorEncoder[motorB];
// WheelCircumferenceMM is in millimeters, so, multiply inches by 25.4 to convert to mm.
targetDegrees = 360 * turnRotations; // Num of Degrees to turn
bFloatDuringInactiveMotorPWM = false; // brake
targetDegrees *= direction;
targetDegrees += rotationSensor; // Add these degrees to the current rotation sensor
checkForEncoderError(targetDegrees); // Setting a MotorEncoder target to an invalid number may result in a logic error. Better check first.
nMotorEncoderTarget[motorB] = targetDegrees; // This is the new target degree to achieve
if(direction == BACKWARD) { // If going backward, then check rotation sensor >= target
while(nMotorRunState[motorB] != runStateIdle && nMotorRunState[motorB] != runStateHoldPosition && nMotorEncoder[motorB] >= targetDegrees) {
if(IsFollowingLine) { FollowLinePID(); }
}
} else { // If going forkward, then check rotation sensor <= target
while(nMotorRunState[motorB] != runStateIdle && nMotorRunState[motorB] != runStateHoldPosition && nMotorEncoder[motorB] <= targetDegrees) {
if(IsFollowingLine) { FollowLinePID(); }
}
}
}
void stopAfterRotations(float inches) { untilRotations(inches); stopMoving(); }
void accelerateOverDistance(float inches, int power) {
long rotationSensor = 0;
long rotationSensor2 = 0;
long rotationSensorStart = 0;
long rotationSensorStart2 = 0;
float targetDegrees;
if(direction == STOPPED) beginDrivingForward(18);
if(direction == FORWARD) { // If going forward, then check rotation sensor <= target
motor[motorB] = 18; // Starting Power Level
} else {
motor[motorB] = -18; // Starting Power Level
}
float powerIncrement = (power-20) / 10;
float incrementAtDegree;
rotationSensor = nMotorEncoder[motorB];
// WheelCircumferenceMM is in millimeters, so, multiply inches by 25.4 to convert to mm.
targetDegrees = 360 * inches * 25.4 / wheelCircumferenceMM * gearRatio; // Num of Degrees to turn
bFloatDuringInactiveMotorPWM = false; // brake
if(direction == BACKWARD) { targetDegrees*= -1; } // Direction is reverse if <0
targetDegrees += rotationSensor; // Add these degrees to the current rotation sensor
checkForEncoderError(targetDegrees); // Setting a MotorEncoder to an invalid number may result in an error. Better check first.
incrementAtDegree = rotationSensor;
nMotorEncoderTarget[motorB] = targetDegrees; // This is the new target degree to achieve
if(direction == FORWARD) { // If going forward, then check rotation sensor <= target
while(nMotorRunState[motorB] != runStateIdle && nMotorRunState[motorB] != runStateHoldPosition && nMotorEncoder[motorB] <= targetDegrees) {
if(nMotorEncoder[motorB] > incrementAtDegree) { incrementAtDegree+=36; motor[motorB] += powerIncrement; }
if(IsFollowingLine) { FollowLinePID(); }
}
} else { // If going backward, then check rotation sensor >= target
while(nMotorRunState[motorB] != runStateIdle && nMotorRunState[motorB] != runStateHoldPosition && nMotorEncoder[motorB] >= targetDegrees) {
if(nMotorEncoder[motorB] < incrementAtDegree) { incrementAtDegree-=36; motor[motorB] -= powerIncrement; }
if(IsFollowingLine) { FollowLinePID(); }
}
}
}
void stopAfterDistanceWithAcceleration(float inches, int power) { accelerateOverDistance(inches, power); stopMoving(); }
void untilTouch() {
while(SensorValue(touchSensor) == 0) { // Loop until the button is pressed
if(direction == FORWARD) { // If going forward, then check rotation sensor <= target
if(IsFollowingLine) { FollowLinePID(); }
} else { // If going backward, then check rotation sensor >= target
if(IsFollowingLine) { FollowLinePID(); }
}
}
}
void stopOnTouch() { untilTouch(); stopMoving(); }
void untilRelease() {
while(SensorValue(touchSensor) == 1) { // Loop until the button is pressed
}
}
//////////////////////////////////////////////////////////////////////
//
// Special Functions
//
float rotations(float tempRotations) { // Shortcut to convert Rotations into degrees
return tempRotations * 360; // May be useful when you want to specify a certain number of
} // rotations and the other function requires degrees
// For example: accessoryDown(rotations(5)); moves accessory for 5 rotations
void align(int dirRotation) {
IsFollowingLine = false; // Just driving forward is not following any line
nSyncedMotors = synchBC; // motor B is the master, motor C is the follower
SensorType[S2] = sensorLightActive; // Both sensors are used in this alignment function.
SensorType[S3] = sensorLightActive; // Make sure you turn the light 'on'
// Drive to the line
if(direction == STOPPED) { direction = FORWARD; } // If you were stopped, then let's go forward.
if(direction == (FORWARD * mDir)) {
motor[mMotor] = powerLevel; // Only the master motor needs power since the other moter is synched to the master
} else {
motor[mMotor] = powerLevelRev; // Only the master motor needs power since the other moter is synched to the master
}
// Enter a loop and stay there as long as the light value is greater than % in variable lineIsAtLevel
while(getLightPercent() > lineIsAtLevel) { }
// Stop moving now. Prepare robot to turn in place.
//motor[motorB] = 0; // turn the motor off.
//motor[motorC] = 0; // turn the motor off.
float criteria1,criteria2,lightLevelPortion;
lightLevelPortion = (float) lineIsAtLevel / 100;
criteria1 = tLowLight + ((tHighLight - tLowLight) * lightLevelPortion);
criteria2 = tLowLight2 + ((tHighLight2 - tLowLight2) * lightLevelPortion);
long point1, point2;
float targetDegrees;
point1 = nMotorEncoder[motorB]; // Get first rotation sensor reading
if(direction == (FORWARD * mDir)) {
motor[mMotor] = powerLevel; // Only the master motor needs power since the other moter is synched to the master
while(SensorRaw[lightSensorB] > criteria2) { }
point2 = nMotorEncoder[mMotor]; // Get second rotation sensor reading
targetDegrees = (point1 + point2) / 2; // Make the robot have a slight bias toward point2. This will
// place the robot in a better position for line following.
motor[mMotor] = powerLevelRev; // Only the master motor needs power since the other moter is synched to the master
while(nMotorEncoder[mMotor] >= targetDegrees) { }
} else {
motor[mMotor] = powerLevelRev; // Only the master motor needs power since the other moter is synched to the master
while(SensorRaw[lightSensorA] > criteria1) { }
point2 = nMotorEncoder[mMotor]; // Get second rotation sensor reading
targetDegrees = (point1 + point2) / 2; // Make the robot have a slight bias toward point2. This will
// place the robot in a better position for line following.
motor[mMotor] = powerLevel; // Only the master motor needs power since the other moter is synched to the master
while(nMotorEncoder[mMotor] <= targetDegrees) { }
}
// Prepare the robot to turn in place
motor[motorB] = 0; // turn the motor off.
motor[motorC] = 0; // turn the motor off.
nSyncedTurnRatio = -100; //motors move in opposite directions of one another
if(dirRotation == COUNTERCLOCKWISE) {
motor[motorB] = powerLevelRev * .6; //turns the motor on at specified power
motor[motorC] = powerLevel * .6; //turns the motor on at specified power
} else {
motor[motorB] = powerLevel * .6; //turns the motor on at specified power
motor[motorC] = powerLevelRev * .6; //turns the motor on at specified power
}
if(direction == (FORWARD * mDir)) {
while(SensorRaw[lightSensorB] > criteria2) { }
} else {
while(SensorRaw[lightSensorA] > criteria1) { }
}
motor[motorB] = 0; // turn the motor off.
motor[motorC] = 0; // turn the motor off.
nSyncedMotors = synchNone; // de-synch the motors
}
void accessoryToDegree(float turnDegrees, int tempPower) {
powerLevelAccessory = tempPower;
float targetDegrees = (turnDegrees - nMotorEncoder[motorA]) % 360;
Motor(motorA,(float) targetDegrees, FORWARD);
}
void moveAccessoryUp(float turnDegrees, int tempPower) {
long rotationSensor;
bFloatDuringInactiveMotorPWM = false; // Use the Brakes
int OldTime = time100[T1];
motor[motorA] = tempPower;
rotationSensor = nMotorEncoder[motorA];
if(turnDegrees < 0) {
while(true) {
wait10Msec(10);
if(abs(rotationSensor - nMotorEncoder[motorA]) < 5) { break; }
rotationSensor = nMotorEncoder[motorA];
}
motor[motorA] = 0;
wait10Msec(10);
motorError[convertMotorToInt(motorA)] = 0; // Clear the error now that the motor has stalled. Don't want this error affecting the next action.
} else {
float targetDegrees = ((turnDegrees + nMotorEncoder[motorA]) - motorError[convertMotorToInt(motorA)]);
while(nMotorEncoder[motorA] <= targetDegrees - ((float)tempPower * .6)) { }
motor[motorA] = 0;
wait10Msec(10);
motorError[convertMotorToInt(motorA)] = nMotorEncoder[convertMotorToInt(motorA)] - targetDegrees;
}
bFloatDuringInactiveMotorPWM = false; // coast or float
// nMotorEncoder[motorA] = 0;
wait10Msec(2);
}
void moveAccessoryDown(float turnDegrees, int tempPower) {
long rotationSensor;
bFloatDuringInactiveMotorPWM = false; // Use the Brakes
int OldTime = time100[T1];
motor[motorA] = -1 * tempPower;
rotationSensor = nMotorEncoder[motorA];
if(turnDegrees < 0) {
while(true) {
wait10Msec(10);
if(abs(rotationSensor - nMotorEncoder[motorA]) < 5) { break; }
rotationSensor = nMotorEncoder[motorA];
}
motor[motorA] = 0;
wait10Msec(10);
motorError[convertMotorToInt(motorA)] = 0; // Clear the error now that the motor has stalled. Don't want this error affecting the next action.
} else {
float targetDegrees = ((nMotorEncoder[motorA] - turnDegrees) - motorError[convertMotorToInt(motorA)]);
while(nMotorEncoder[motorA] >= targetDegrees + ((float)tempPower * .6)) { }
motor[motorA] = 0;
wait10Msec(10);
motorError[convertMotorToInt(motorA)] = nMotorEncoder[convertMotorToInt(motorA)] - targetDegrees;
}
bFloatDuringInactiveMotorPWM = false; // Keep Brake on
wait10Msec(2);
}
void forwardToLine() {
beginDrivingForward(powerLevel/2);
SensorType[S3] = sensorLightActive; // Make sure you turn the light 'on'
SensorType[S2] = sensorLightActive; // Make sure you turn the light 'on'
while(getLightPercentA() > lineIsAtLevel && getLightPercentB() > lineIsAtLevel) { }
if(getLightPercentA() > lineIsAtLevel) {
nSyncedMotors = synchBC; // No motor synch
bFloatDuringInactiveMotorPWM = false; // Enable braking
nSyncedTurnRatio = -100; // motors move in opposite directions of one another
motor[motorB]=powerLevelRev/2;
while(getLightPercentA() > lineIsAtLevel) { // Loop until the button is pressed
}
stopMoving();
} else {
if(getLightPercentB() > lineIsAtLevel) {
nSyncedMotors = synchCB; // No motor synch
bFloatDuringInactiveMotorPWM = false; // Enable braking
nSyncedTurnRatio = -100; // motors move in opposite directions of one another
motor[motorC]=powerLevelRev/2;
while(getLightPercentB() > lineIsAtLevel) { // Loop until the button is pressed
}
stopMoving();
}
}
SensorType[S3] = sensorLightInactive; // Make sure you turn the light 'off'
SensorType[S2] = sensorLightInactive; // Make sure you turn the light 'off'
stopMoving(); // Stop moving now
resetRotationSensor(motorB, motorC);
}
void beginBackIntoWall(int tempPower) {
beginDrivingBackward(tempPower);
while(SensorValue(touchSensor) == 0 && SensorValue(touchSensorB) == 0) { // Loop until the button is pressed
if(direction == FORWARD) { // If going forward, then check rotation sensor <= target
if(IsFollowingLine) { FollowLinePID(); }
} else { // If going backward, then check rotation sensor >= target
if(IsFollowingLine) { FollowLinePID(); }
}
}
if(SensorValue(touchSensorB) == 0) {
while(SensorValue(touchSensorB) == 0) { // Loop until the button is pressed
nSyncedMotors = synchNone; // No motor synch
bFloatDuringInactiveMotorPWM = false; // Enable braking
if(direction == BACKWARD) { // If going forward, then check rotation sensor <= target
motor[motorC]=tempPower;
} else {
motor[motorC]=tempPower * -1;
}
}
stopMoving();
}
if(SensorValue(touchSensor) == 0) {
while(SensorValue(touchSensor) == 0) { // Loop until the button is pressed
nSyncedMotors = synchNone; // No motor synch
bFloatDuringInactiveMotorPWM = false; // Enable braking
if(direction == BACKWARD) { // If going forward, then check rotation sensor <= target
motor[motorB]=tempPower;
} else {
motor[motorB]=tempPower * -1;
}
}
stopMoving();
}
beginDrivingBackward(tempPower);
stopAfterDistance(0.20);
resetRotationSensor(motorB, motorC);
}
void backIntoWall() {
beginBackIntoWall(powerLevel);
}
void backIntoWall(int tempPower) {
beginBackIntoWall(tempPower);
}
////////////////////////////////////////////////////////////////////////////////
//
// Menu System Primitives
//
////////////////////////////////////////////////////////////////////////////////
#define MAX_MENU_ITEMS 20
#define FUNCTION_1 10
#define FUNCTION_15 15
#define FUNCTION_2 20
#define FUNCTION_25 25
#define FUNCTION_3 30
#define FUNCTION_35 35
#define FUNCTION_4 40
#define FUNCTION_45 45
#define FUNCTION_5 50
#define FUNCTION_6 60
#define FUNCTION_7 70
#define FUNCTION_8 80
#define FUNCTION_9 90
string menu_prompts[MAX_MENU_ITEMS];
int menu[MAX_MENU_ITEMS];
int lastMenuItem = 0;
int scrollTopMenuItem = 0;
int cursorAtMenuItem = 0;
int selectedItem = 0;
int addMenuItem(string *promptStr, int retVal)
{
menu_prompts[lastMenuItem++] = *promptStr;
return retVal;
}
void showMenu()
{
display(1, "-- Missions --");
int cdLine = 0;
for(cdLine = 0; cdLine < 6; cdLine++)
{
nxtDisplayTextLine(cdLine+2, " %s", menu_prompts[cdLine+scrollTopMenuItem]);
}
}
void clearDisplay()
{
int cdLine = 1;
for(cdLine = 1; cdLine <= 8; cdLine++)
{
nxtDisplayTextLine(cdLine, " ");
}
}
void showCursor(int menuItemNum) {
int putOnLine = (menuItemNum - scrollTopMenuItem) + 2;
nxtDisplayTextLine(putOnLine, ">>%s", menu_prompts[menuItemNum]);
}
void adjustMenuItem(const bool bIncrement) {
if(bIncrement)
{
cursorAtMenuItem = ++cursorAtMenuItem % lastMenuItem;
scrollTopMenuItem = (cursorAtMenuItem >= scrollTopMenuItem+6) ? ++scrollTopMenuItem: ((cursorAtMenuItem==0)? 0: scrollTopMenuItem);
}
else
{
cursorAtMenuItem = --cursorAtMenuItem % lastMenuItem;
cursorAtMenuItem = (cursorAtMenuItem < 0) ? (lastMenuItem-1): cursorAtMenuItem;
scrollTopMenuItem = (cursorAtMenuItem < scrollTopMenuItem) ? ((scrollTopMenuItem>0)?--scrollTopMenuItem : 0) : ((cursorAtMenuItem >= scrollTopMenuItem+6) ? cursorAtMenuItem-5: scrollTopMenuItem);
}
showMenu();
showCursor(cursorAtMenuItem);
PlaySoundFile("! Click.rso");
wait(.6);
}
int startMenu()
{
cursorAtMenuItem = 0;
showCursor(cursorAtMenuItem);
while(nNxtButtonPressed != kEnterButton) { // Wait Here
switch(nNxtButtonPressed)
{
case kLeftButton: adjustMenuItem(false); break;
case kRightButton:adjustMenuItem(true); break;
default:
wait(0.1);
}
}
PlaySoundFile("! Click.rso");
return cursorAtMenuItem;
}
////////////////////////////////////////////////////////////////////////////////
//
// Menu System Usage
//
////////////////////////////////////////////////////////////////////////////////
/*----
//The menu is stored in an array structure. The syntax for creating menu items is as follows:
menu[lastMenuItem] = addMenuItem("Calibrate Light", FUNCTION_1);
menu[lastMenuItem] = addMenuItem("Round the Track", FUNCTION_6);
menu[lastMenuItem] = addMenuItem("Drive To Center", FUNCTION_2);
showMenu(); // The menu is shown
selectedItem = startMenu(); // An item is selected
switch (menu[selectedItem]) // Now switch based on the menu value of the selected item
{
case FUNCTION_1: calibrate(); break; // With a break, only this function will be executed.
case FUNCTION_6: roundTrack(); // Without a break, every subsequent function will be executed.
case FUNCTION_2: toCenter();
case FUNCTION_3: locateOtherRobot();
case FUNCTION_4: crash();
}
-----*/
/////////////////////////////////////////////////////////////////////////////////////////////
//
// Define the Overloaded Functions
//
void pause() {
waitForTouchSensor();
}
void driveForward() { // Overloaded function: 0 parameters
beginDrivingForward(powerLevel);
}
void driveForward(float inches) { // Overloaded function: 1 parameter
beginDrivingForward(powerLevel);
stopAfterDistance(inches);
}
void driveForward(float inches, int power) { // Overloaded function: 2 parameters
beginDrivingForward(power);
stopAfterDistance(inches);
}
void driveBackward() { // Overloaded function: 0 parameters
beginDrivingBackward(powerLevel);
}
void driveBackward(float inches) { // Overloaded function: 1 parameter
beginDrivingBackward(powerLevel);
stopAfterDistance(inches);
}
void driveBackward(float inches, int power) { // Overloaded function: 2 parameters
beginDrivingBackward(power);
stopAfterDistance(inches);
}
void followLine(int thisSide, float inches) {
followLine(thisSide);
stopAfterDistance(inches);
}
void followWall(int thisSide) { // Overloaded function: 1 parameters
beginFollowingWall(thisSide, powerLevel);
}
void followWall(int thisSide, float inches) { // Overloaded function: 2 parameters
beginFollowingWall(thisSide, powerLevel);
stopAfterDistance(inches);
}
void followWall(int thisSide, float inches, int power) { // Overloaded function: 3 parameters
beginFollowingWall(thisSide, power);
stopAfterDistance(inches);
}
void wait() { wait(1); } // Overload the Wait function to create a default without parameters
// Overloaded function: 2 parameters
void gradualTurnClockwise(float turnDegrees, float turnRadiusIN) {
gradualTurnClockwise(turnDegrees, turnRadiusIN, powerLevel);
}
void gradualTurnCounterClockwise(float turnDegrees, float turnRadiusIN) {
gradualTurnCounterClockwise(turnDegrees, turnRadiusIN, powerLevel);
}
void swingTurnClockwise(float turnDegrees) { // Overloaded function: 1 parameter
swingTurnClockwise(turnDegrees, powerLevel);
}
void swingTurnCounterClockwise(float turnDegrees) {// Overloaded function: 1 parameter
swingTurnCounterClockwise(turnDegrees, powerLevel);
}
void swingTurn(int dirRotation, float turnDegrees, int tempPower) { // new function: swingTurn()
if(dirRotation == COUNTERCLOCKWISE) {
swingTurnCounterClockwise(turnDegrees, tempPower);
} else {
swingTurnClockwise(turnDegrees, tempPower);
}
}
void swingTurn(int dirRotation, float turnDegrees) { // Overloaded function: 2 parameters
swingTurn(dirRotation, turnDegrees, powerLevel);
}
void pointTurnClockwise(float turnDegrees) { pointTurnClockwise(turnDegrees, powerLevel); }
void pointTurnCounterClockwise(float turnDegrees) { pointTurnCounterClockwise(turnDegrees, powerLevel); }
void pointTurn(int dirRotation, float turnDegrees, int tempPower) { // new function: pointTurn()
if(dirRotation == COUNTERCLOCKWISE) {
pointTurnCounterClockwise(turnDegrees, tempPower);
} else {
pointTurnClockwise(turnDegrees, tempPower);
}
}
void pointTurn(int dirRotation, float turnDegrees) { // Overloaded function: 2 parameters
pointTurn(dirRotation, turnDegrees, powerLevel);
}
void accessoryUp() {
if(aDir == FORWARD) { moveAccessoryUp(-1, 25); } else { moveAccessoryDown(-1, 25); }
}
void accessoryDown() {
if(aDir == FORWARD) { moveAccessoryDown(-1, 25); } else { moveAccessoryUp(-1, 25); }
}
void accessoryUp(float turnDegrees) {
if(aDir == FORWARD) { moveAccessoryUp(turnDegrees, 25); } else { moveAccessoryDown(turnDegrees, 25); }
}
void accessoryUp(float turnDegrees, int tempPower) {
if(aDir == FORWARD) { moveAccessoryUp(turnDegrees, tempPower); } else { moveAccessoryDown(turnDegrees, tempPower); }
}
void accessoryDown(float turnDegrees) {
if(aDir == FORWARD) { moveAccessoryDown(turnDegrees, powerLevelAccessory); } else { moveAccessoryUp(turnDegrees, powerLevelAccessory); }
}
void accessoryDown(float turnDegrees, int tempPower) {
if(aDir == FORWARD) { moveAccessoryDown(turnDegrees, tempPower); } else { moveAccessoryUp(turnDegrees, tempPower); }
}
void accessoryToDegree(float turnDegrees) {
accessoryToDegree(turnDegrees, powerLevelAccessory);
}
// This is a generic function to run both initialization functions.
// The initialization functions could be run individually for smaller programs.
// This Startup block must be included at the end of the file so that the
// references to functions shown below do not generate compiler errors which would happen
// if the function is referenced before it is actually defined. Functions are defined first. Then referenced.
void startup() {
//sFileNameRD = "RobotDim.txt";
InitMove(); // Loads the physical characteristics of the robot
initLightSensor(); // Loads the light sensor ranges to be used for identifying and following lines
resetRotationSensor(motorA, motorB, motorC);
wheelCircumferenceMM = wheelDiameterMM * PI; // Formula. NOT changeable.
if(1==2) { // This block is NOT executed. It is included here only so that warnings
// regarding Unreferenced Functions are not shown.
accessoryUp(); // Again. This block is not executed. It is referenced here so that
accessoryDown(rotations(3)); // compiler warnings are suppressed.
accessoryToDegree(310);
display(0,"");
wait(0);
startup();
waitForOrangeButton();
setPowerLevel(0); setDefaultSpeed(0); setAccessoryPowerLevel(0);
resetRotationSensor(motorA);
getLightDiffPortion();
getLightPercentA();
getLightPercentB();
accelerateOverDistance(0,0);
driveBackward();
driveBackward(0);
driveBackward(0,0);
driveForward();
driveForward(0);
driveForward(0,0);
followWall(0);
followWall(0,0);
followWall(0,0,0);
backIntoWall();
Motor(0,0,0);
gradualTurnCounterClockwise(0,0);
gradualTurnClockwise(0,0);
swingTurnCounterClockwise(0);
swingTurnClockwise(0);
swingTurn(CLOCKWISE,90);
stopOnLine();
untilDistance(0);
stopAfterRotations(0);
stopAfterDistance(0);
stopOnTouch();
stopIfSonarLessThan(0);
isSonarLessThan(0);
isSonarGreaterThan(0);
robotSees(0);
stopIfRobotSees(0);
stopAfterDistanceWithAcceleration(0,0);
untilTouch();
untilRelease();
untilSonarGreaterThan(0);
untilRobotSees(0);
untilRobotSees(0,0);
waitForTouchSensor();
pause();
align(0);
followLine(LEFTSIDE);
calibrateLightSensor();
createLightCalibrationFile(0);
createRobotDimensionsConfig(0);
configureRobotDimensions(centerOfWheelToCenterOfRobotMM,wheelDiameterMM,gearRatio);
configureWheelToCenterOfRobotMM(centerOfWheelToCenterOfRobotMM);
configureWheelDiameterMM(wheelDiameterMM);
// ConfigureRobotDimensions(centerOfWheelToCenterOfRobotMM,wheelDiameterMM,gearRatio,FORWARD);
sideOfLine = MIDDLE; // Initialized here only to avoid a compiler warning that they are not used anywhere else.
sideOfLine = RIGHTSIDE;
sideOfLine = LEFTSIDE;
}
}
