Our Function Library (Updated Spring 2015)
A
function library is a set of functions, variables, constants, and algorithms that are
included with the basic RobotC programs. The functions and variables in the library can then be referenced and called upon as needed in your program.
The purpose of keeping these functions and variables in one file is to make your programs more modular. Many programs can refer to and use the same function library. Also, if these functions and variables are stored in a file separate from your program, then you do not have to include all of the code below in order to access it. You can simply use one line of code to tell the compiler to include the contents of the entire library into your program. The compilter will do that at the time your program is compiled.
Structure
Most function libraries have a structure to help make them easier to understand by programmers. The format of this structure is one where the constants and variables are declared (for the most part) at the beginning of the file. This is also somewhat necessary since a constant or variable cannot be used by any part of the program before it has been defined (declared).
Declaration of Constants and Variables
The statements below show the definitions of various constants and variables. Most are assigned a value in their declaration.
//////////////////////////////////////////////////////////////////////
//
// Define constants and variables to be used in the functions below
//
// Internal constants used to write more readable code.
const int RIGHTSIDE = 1; // Line following constants used to tell which side of line to follow
const int MIDDLE = 0; //
const int LEFTSIDE = -1; //
const int STOPPED = 0;
int FORWARD = 1; // General navigation used to keep track of which direction the robot is going
int BACKWARD = -1; // These are declared as variables so they can be reversed for certain robot designs
int mDir = FORWARD; // Orientation of the motors. If motors are reversed,
// the config function will change this value
const int CLOCKWISE = 1;
const int COUNTERCLOCKWISE = -1;
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
int direction = STOPPED; // Initial direction of the robot is set to Stopped
int sideOfLine = RIGHTSIDE; // Initialize a variable to keep track of which side of the line the robot is following
bool keepGoing = false; // Some functions allow for the robot to keep moving when true;
// If false, robot will stop after each action
int lineIsAtLevel = 20; // Percentage of light that is used to determine presence of a line.
float motorError[] = {0, 0, 0};// 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.Primitives
Certain functions may perform very basic (
primitive) operations and are often referred to in other programs/functions that follow. For this reason,
primitive functions usually follow the first declaration of constants/variables.
//////////////////////////////////////////////////////////////////////
//
// Define Primitive functions that may be referenced elsewhere
//
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,"* *");
}
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,"* *");
}Variables Defined Next to the Functions That Use Them
While it is possible to declare all variables at the top of the file, it sometimes is easier to read and understand the program if variables are declared next to the functions that use them.
//////////////////////////////////////////////////////////////////////
//
// 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.
createRobotDimensionsConfig()
This is an internal primitive function. It is not referenced by the programmer. Rather, there are one or more other functions that call this one. In this case, this function is called by
startup() which is a function you include at the beginning of your RobotC programs. This function begins the process of initializing the robot and getting it ready for action.
Purpose
The purpose of
createRobotDimensionsConfig() is to create a text file that will contain the default dimensions of the robot.
Why store the dimensions of the robot's in a file? Good question. The answer is: so you can write programs that do not have to specify the dimensions of the robot. If the dimensions of the robot are stored in a file on the robot itself, then the program will simply refer to those dimensions as needed. This saves you the trouble of constantly having to refer to them in your program.
Where is this file that contains the robot's dimensions?The name of the file is defined in a variable above. The variable is labeled:
sFileNameRD which is assigned the value: "
RobotDimensions.txt"
How is the file created?The file is created in the function below where it shows:
OpenWrite( ... ). This tells the computer to open the file for "writing" and four floating point numbers are written to it. The file is closed and a sound is played when done.
What values are written to the file?
- centerOfWheelToCenterOfRobotMM
- wheelDiameterMM
- wheelCircumference
- gearRatio
These four dimensions of the robot are assigned initial values before the file is written. The initial values should be the values for your team's robot that you are building for the season. This assumes that everyone's robot has the same dimensions. If your robot's dimensions differ from these, you can either update this numbers to reflect your robot's dimensions. Or, you can declare your robot's dimensions at the beginning of your program.
//////////////////////////////////////////////////////////////////////
//
// Functions used to Configure Robot Dimensions
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"
float 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
// c = the gear that connects to the motor
// b = the gear in the middle
// 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");
}Functions that Allow You to Change the Default Values
When the program is started, a set of default values for the robot's dimensions are retrieved. If your robot differs from these default values, you can update the internal variables to reflect your robot's true dimensions through the use of these functions.
void configureRobotDimensions(float WheelToCenterOfRobot, float WheelDiameter, float DriveGearRatio, int RobotForwardIs ) {
centerOfWheelToCenterOfRobotMM = WheelToCenterOfRobot;
wheelDiameterMM = WheelDiameter;
wheelCircumferenceMM = wheelDiameterMM * PI; // Formula. NOT changeable.
gearRatio = DriveGearRatio;
mDir=RobotForwardIs;
}
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) {
configureRobotDimensions(WheelToCenterOfRobot, WheelDiameter, DriveGearRatio, FORWARD);
}
// Overload function for default gear ratio and motor orientation
void configureRobotDimensions(float WheelToCenterOfRobot, float WheelDiameter) {
configureRobotDimensions(WheelToCenterOfRobot, WheelDiameter, 1.0, FORWARD);
}Function Overloading
Did you notice above that there appeared to be multiple definitions of the same function: configureRobotDimensions()Why was that? Why wasn't one definition good enough?The "compiler" looks at your program, and based on how you use the function (with one, two, three, or four parameters) it will then choose from the list of defined variations of that function the one that best matches how you used it in your program.
So, for example, you can use the following in your program:
configureRobotDimensions(85.00, 43.2)and
configureRobotDimensions(85.00, 43.2, 2.5)The computer will know that the second one is the one where the GearRatio was specified and it will use that function to set the dimensions including the GearRaio.
Working With Light Sensors
Your robot may need to use light sensors for line following. The actual light sensor itself returns a raw value (ranges between 0 and 1000). This number may not be very useful if it the conditions of the environment change from one setting to another. For example, if your practice (training) area uses florescent lighting in a low hung ceiling, your light sensors may give certain values. These values may differ if in the competion gymnasium the lighting is provided by high voltage arc lamps. For this reason, it may be a good idea to
calibrate the light sensors for the environment they will be used in. Each time the environment changes, you would need to
re-calibrate the light sensors.
////////////////////////////////////////////////////////////////////////////////////
//
// Define the variables and constants to be used with LineFollowing.
//
// This robot will have two light sensors. Each sensor has a slightly different range.
// Some of these differences can be as much as 5%. To account for the differences, a separate
// set of ranges is created for each light sensor.
const string sFileNameLR = "LightRange.txt";
long tLowLight = 300;
long tHighLight = 600;
long tLowLight2 = 300;
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;
if((long) SensorRaw[lightSensorB] > tHighLight2) { tHighLight2 = (long) SensorRaw[lightSensorB]; }
if((long) SensorRaw[lightSensorB] < tLowLight2) { tLowLight2 = (long) SensorRaw[lightSensorB]; }
if((long) SensorRaw[lightSensorA] > tHighLight) { tHighLight = (long) SensorRaw[lightSensorA]; }
if((long) SensorRaw[lightSensorA] < tLowLight) { tLowLight = (long) SensorRaw[lightSensorA]; }
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 getLightPercent() { return 100 * getLightPortion(); }
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;
}PID Controller
The PID controller is implemented using two separate functions. One function performs the PID function (called: FollowLinePID) while the other function setups the main variables to be used (called: followLine).
The followLine function takes one parameter to specify the side of the line to follow (LEFT or RIGHT). Notice how the followLine function does not include any parameters for distance or power level. This is because there are other functions which the robot should use when determining distances. If you are following a line and you want the robot to stop after 4 inches, then you would get the robot to follow the line using the followLine(); function and then use the command
stopAfterDistance(4);.
Each function in the library that checks for distance travelled or for distance to an object will also utilize the FollowLinePID() function if the robot happens to be following a line at the time.
//////////////////////////////////////////////////////////////////////
//
// 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 = 1; // Used as a multiplier to change the power to the motors from + to - if going in reverse.
if(direction == BACKWARD) { dFactor = -1;}// If not going forward, then you must be in reverse.
float variablePower = powerLevel * 0.75; // variablePower should be a portion of the default powerLevel.
float Kp = 0; // Using the "PID minus the ID" formula
Kp = ((getLightPercent() - 50) / 50) * variablePower; // Calculates Kp as a percentage of the variablePower amount.
if(sideOfLine == RIGHTSIDE) { Kp *= -1; } // Switch the sign of Kp if you want to follow the other side of the line.
motor[motorB] = dFactor * (powerLevel + Kp) * mDir; // Add Kp to motor1 whether going forward or backward
motor[motorC] = dFactor * (powerLevel - Kp) * mDir; // Subtract Kp from motor2 whether going forward or backward
}// 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()
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; } // 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'
SensorType[S2] = sensorLightInactive;
} else {
if(SensorType[S2] == sensorLightInactive) { SensorType[S2] = sensorLightActive; } // Make sure you turn the light 'on'
SensorType[S3] = sensorLightInactive;
}
FollowLinePID();
}Moving
//////////////////////////////////////////////////////////////////////
//
// Primitive 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) {
direction = FORWARD; // Set the direction so other functions know which way we are going
IsFollowingLine = false; // Just driving forward is not following any line
nSyncedMotors = synchBC; // motor B is the master, motor C is the follower
// Only the master motor needs power since the other moter is synched to the master
motor[motorB] = direction * tempPower * mDir;
}
void beginDrivingBackward(int tempPower) {
direction = BACKWARD; // Set the direction so other functions know which way we are going
IsFollowingLine = false; // Just driving backward is not following any lines
nSyncedMotors = synchBC; // motor B is the master, motor C is the follower
// Only the master motor needs power since the other moter is synched to the master
motor[motorB] = direction * tempPower * mDir;
}
void beginFollowingWall(int thisSide, int tempPower) {
if(direction == STOPPED) beginDrivingForward(tempPower);
IsFollowingLine = false; // Just driving forward is not following any line
nSyncedMotors = synchBC; // motor B is the master, motor C is the follower
// Only the master motor needs power since the other moter is synched to the master
nSyncedTurnRatio = thisSide * 97; // One Motor will be 90% the value of the other
motor[motorB] = direction * tempPower * mDir;
}Turning
//////////////////////////////////////////////////////////////////////
//
// 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.
//
void pointTurnClockwise(float turnDegrees, int tempPower) {
float turningCircumferenceMM = (2 * centerOfWheelToCenterOfRobotMM * PI);
float wheelRotationsPerRobotTurn = turningCircumferenceMM / wheelCircumferenceMM;
float wheelDegreesOfTurningNeeded = wheelRotationsPerRobotTurn * turnDegrees;
float targetDegrees = wheelDegreesOfTurningNeeded * gearRatio;
targetDegrees = targetDegrees + nMotorEncoder[motorB];
nSyncedMotors = synchBC; //motor B is the master, motor C is the slave
if(keepGoing) {
bFloatDuringInactiveMotorPWM = true; // coast or float
} else {
bFloatDuringInactiveMotorPWM = false; // brake
}
nSyncedTurnRatio = -100; // motors move in opposite directions of one another
motor[motorB] = tempPower; // turns the motor on at specified power
while(nMotorEncoder[motorB] <= targetDegrees) { }
if(!keepGoing) motor[motorB] = 0; // turn the motor off.
bFloatDuringInactiveMotorPWM = false; // brake
IsFollowingLine = false; // Turning means you are not following any line
}void swingTurnClockwise(float turnDegrees, int tempPower) {
float turningCircumferenceMM = (4 * centerOfWheelToCenterOfRobotMM * PI);
float wheelRotationsPerRobotTurn = turningCircumferenceMM / wheelCircumferenceMM;
float wheelDegreesOfTurningNeeded = wheelRotationsPerRobotTurn * turnDegrees;
float targetDegrees = wheelDegreesOfTurningNeeded * gearRatio;
if(direction == STOPPED) beginDrivingForward(powerLevel);
if(direction == FORWARD) {
targetDegrees = targetDegrees + nMotorEncoder[motorB];
nSyncedMotors = synchNone; // de-synch the motors
if(keepGoing) {
bFloatDuringInactiveMotorPWM = true; // coast or float
} else {
bFloatDuringInactiveMotorPWM = false;// brake
}
motor[motorB] = tempPower; // turns the motor on at specified power
while(nMotorEncoder[motorB] <= targetDegrees) { }
if(!keepGoing) motor[motorB] = 0; // turn the motor off.
} else {
targetDegrees = nMotorEncoder[motorC] - targetDegrees;
nSyncedMotors = synchNone; // de-synch the motors
if(keepGoing) {
bFloatDuringInactiveMotorPWM = true; // coast or float
} else {
bFloatDuringInactiveMotorPWM = false;// brake
}
motor[motorC] = tempPower; // turns the motor on at specified power
while(nMotorEncoder[motorC] <= targetDegrees) { }
if(!keepGoing) motor[motorC] = 0; // turn the motor off.
}
bFloatDuringInactiveMotorPWM = false; // brake
IsFollowingLine = false; // Turning means you are not following any line
}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
}
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 == FORWARD) {
targetDegrees = nMotorEncoder[motorB] + targetDegrees;
nSyncedMotors = synchBC; // motor B is the master, motor C is the slave
nSyncedTurnRatio = turnRatio; // turn ratio is set after the motors are synced
motor[motorB] = tempPower; // turns the motor on at specified power
while(nMotorEncoder[motorB] <= targetDegrees) { } //continue until the Target position is reached
if(!keepGoing) motor[motorB] = 0; // turn the motor off.
} else {
targetDegrees = nMotorEncoder[motorC] - targetDegrees;
nSyncedMotors = synchCB; // motor B is the master, motor C is the slave
nSyncedTurnRatio = turnRatio;
motor[motorC] = -1 * tempPower; // turns the motor on at specified power
while(nMotorEncoder[motorC] >= targetDegrees) { } //continue until the Target position is reached
if(!keepGoing) motor[motorC] = 0; // turn the motor off.
}
bFloatDuringInactiveMotorPWM = false; // brake
IsFollowingLine = false; // Turning means you are not following any line
}void pointTurnCounterClockwise(float turnDegrees, int tempPower) {
float turningCircumferenceMM = (2 * centerOfWheelToCenterOfRobotMM * PI);
float wheelRotationsPerRobotTurn = turningCircumferenceMM / wheelCircumferenceMM;
float wheelDegreesOfTurningNeeded = wheelRotationsPerRobotTurn * turnDegrees;
float targetDegrees = wheelDegreesOfTurningNeeded * gearRatio;
targetDegrees = targetDegrees + nMotorEncoder[motorC];
nSyncedMotors = synchCB; //motor C is the master, motor B is the slave
if(keepGoing) {
bFloatDuringInactiveMotorPWM = true; // coast or float
} else {
bFloatDuringInactiveMotorPWM = false; // brake
}
nSyncedTurnRatio = -100; //motors move in opposite directions of one another
motor[motorC] = tempPower; //turns the motor on at specified power
while(nMotorEncoder[motorC] <= targetDegrees) { }
//continue to power motorC until the motor nMotorEncoderTarget position is reached
if(!keepGoing) motor[motorC] = 0; // turn the motor off.
bFloatDuringInactiveMotorPWM = false; // brake
IsFollowingLine = false; // Turning means you are not following any line
}void swingTurnCounterClockwise(float turnDegrees, int tempPower) {
float turningCircumferenceMM = (4 * centerOfWheelToCenterOfRobotMM * PI);
float wheelRotationsPerRobotTurn = turningCircumferenceMM / wheelCircumferenceMM;
float wheelDegreesOfTurningNeeded = wheelRotationsPerRobotTurn * turnDegrees;
float targetDegrees = wheelDegreesOfTurningNeeded * gearRatio;
if(direction == STOPPED) beginDrivingForward(powerLevel);
if(direction == FORWARD) {
targetDegrees = targetDegrees + nMotorEncoder[motorC];
nSyncedMotors = synchNone; // de-synch the motors
if(keepGoing) {
bFloatDuringInactiveMotorPWM = true; // coast or float
} else {
bFloatDuringInactiveMotorPWM = false;// brake
}
motor[motorC] = tempPower; // turns the motor on at specified power
while(nMotorEncoder[motorC] <= targetDegrees) { }
if(!keepGoing) motor[motorC] = 0; // turn the motor off.
} else {
targetDegrees = nMotorEncoder[motorB] - targetDegrees;
nSyncedMotors = synchNone; // de-synch the motors
if(keepGoing) {
bFloatDuringInactiveMotorPWM = true; // coast or float
} else {
bFloatDuringInactiveMotorPWM = false;// brake
}
motor[motorB] = tempPower; // turns the motor on at specified power
while(nMotorEncoder[motorB] <= targetDegrees) { }
if(!keepGoing) motor[motorB] = 0; // turn the motor off.
}
bFloatDuringInactiveMotorPWM = false; // brake
IsFollowingLine = false; // Turning means you are not following any line
}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
}
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 == FORWARD) {
targetDegrees = nMotorEncoder[motorC] + targetDegrees;
nSyncedMotors = synchCB; // motor C is the master, motor B is the slave
nSyncedTurnRatio = turnRatio; // turn ratio is set after the motors are synced
motor[motorC] = tempPower; // turns the motor on at specified power
while(nMotorEncoder[motorC] <= targetDegrees) { } // until the Target position is reached
if(!keepGoing) motor[motorC] = 0; // turn the motor off.
} else {
targetDegrees = nMotorEncoder[motorB] - targetDegrees;
nSyncedMotors = synchBC; // motor C is the master, motor B is the slave
nSyncedTurnRatio = turnRatio; // turn ratio is set after the motors are synced
motor[motorB] = -1 * tempPower; // turns the motor on at specified power
while(nMotorEncoder[motorB] >= targetDegrees) { } // until the motor Target position is reached
if(!keepGoing) motor[motorB] = 0; // turn the motor off.
}
bFloatDuringInactiveMotorPWM = false; // brake
IsFollowingLine = false; // Turning means you are not following any line
}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
}
//float initialMotorEncoderValue = nMotorEncoder[thisMotor];
if(dirRotation == BACKWARD) {
targetDegrees = ((nMotorEncoder[thisMotor] - turnDegrees) - motorError[thisMotor]);
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[thisMotor]);
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(100); // 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[thisMotor] = nMotorEncoder[thisMotor] - targetDegrees;
wait10Msec(2);
}Stopping
//////////////////////////////////////////////////////////////////////
//
// 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
}
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
while(getLightPercent() > lineIsAtLevel) { }
stopMoving(); // Stop moving now
SensorType[S3] = sensorLightInactive; // Go ahead and turn the light 'off'
SensorType[S2] = sensorLightInactive; // Go ahead and turn the light 'off'
}
void stopOnLine() { untilLine(); stopMoving(); }Color Sensor
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;
}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;
}void stopIfRobotSees(int seesColor) { untilRobotSees(seesColor); stopMoving(); }
short sonarSensor;
void untilSonarLessThan(float distance_in_inches) {
float distance_in_cm; distance_in_cm = distance_in_inches * 2.54;
while(SensorValue((short) sonarSensor) > distance_in_cm) {
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(); }
}
}
}
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
}
}
void untilDistance(float inches) {
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
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
if(direction == STOPPED) beginDrivingForward(powerLevel);
if(direction == (FORWARD * mDir)) { // If going forward, then check rotation sensor <= target
while(nMotorRunState[motorB] != runStateIdle && nMotorRunState[motorB] != runStateHoldPosition && nMotorEncoder[motorB] <= targetDegrees) {
if(IsFollowingLine) { FollowLinePID(); }
}
} else { // If going backward, then check rotation sensor >= target
while(nMotorRunState[motorB] != runStateIdle && nMotorRunState[motorB] != runStateHoldPosition && nMotorEncoder[motorB] >= targetDegrees) {
if(IsFollowingLine) { FollowLinePID(); }
}
}
}
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
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;
float targetDegrees;
if(direction == STOPPED) beginDrivingForward(18);
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
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
//////////////////////////////////////////////////////////////////////
//
// 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 rotationsvoid accessoryToDegree(float turnDegrees, int tempPower) {
powerLevelAccessory = tempPower;
float targetDegrees = (turnDegrees - nMotorEncoder[motorA]) % 360;
Motor(motorA,(float) targetDegrees, FORWARD);
}
void accessoryUp(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) {
if( time100[T1] - OldTime > 2 ) {
OldTime = time100[T1];
if(abs(rotationSensor - nMotorEncoder[motorA]) < 5) { break; }
rotationSensor = nMotorEncoder[motorA];
}
}
motor[motorA] = 0;
wait10Msec(100);
motorError[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[motorA]);
while(nMotorEncoder[motorA] <= targetDegrees - ((float)tempPower * .6)) { }
motor[motorA] = 0;
wait10Msec(100);
motorError[motorA] = nMotorEncoder[motorA] - targetDegrees;
}
bFloatDuringInactiveMotorPWM = false; // coast or float
// nMotorEncoder[motorA] = 0;
wait10Msec(2);
}void accessoryDown(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) {
if( time100[T1] - OldTime > 2 ) {
OldTime = time100[T1];
if(abs(rotationSensor - nMotorEncoder[motorA]) < 5) { break; }
rotationSensor = nMotorEncoder[motorA];
}
}
motor[motorA] = 0;
wait10Msec(100);
motorError[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[motorA]);
while(nMotorEncoder[motorA] >= targetDegrees + ((float)tempPower * .6)) { }
motor[motorA] = 0;
wait10Msec(100);
motorError[motorA] = nMotorEncoder[motorA] - targetDegrees;
}
bFloatDuringInactiveMotorPWM = false; // Keep Brake on
wait10Msec(2);
}Overloaded Functions Defined
/////////////////////////////////////////////////////////////////////////////////////////////
//
// Define the Overloaded Functions
//
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() {
accessoryUp(-1, 25);
}
void accessoryDown() {
accessoryDown(-1, 25);
}
void accessoryUp(float turnDegrees) {
accessoryUp(turnDegrees, 25);
}
void accessoryDown(float turnDegrees) {
accessoryDown(turnDegrees, powerLevelAccessory);
}
void accessoryToDegree(float turnDegrees) {
accessoryToDegree(turnDegrees, powerLevelAccessory);
}
void resetRotationSensor(int thisMotor) {
nMotorEncoder[thisMotor] = 0;
motorError[thisMotor] = 0; // Clear any motor error
}Startup Function
// 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() {
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 // Not using Sonar in F2012
SensorType[lightSensorA] = sensorLightInactive;
SensorType[lightSensorB] = sensorLightInactive;
SensorType[colorSensor] = sensorLightInactive;
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);
wheelCircumferenceMM = wheelDiameterMM * PI; // Formula. NOT changeable.
}