Our Function Library (Updated Spring 2014)
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.
#pragma systemFile // this eliminates warning for "unreferenced" functions
typedef struct xstruct_config {
// 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; // 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; // 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; // The computer will figure this one.
float gearRatio; // Used as a multiplier to figure out the number of degrees of rotation.
// 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
} struct_config;
struct_config config;
typedef struct xstruct_lineFollower {
////////////////////////////////////////////////////////////////////////////////////
//
// Define the variables and constants to be used with LineFollowing and working with lines.
//
tSensors port;
int color;
int criteria;
int sideOfLine;
} struct_lineFollower;
struct_lineFollower lineFollower; // Line Sensor Configuration Parameterstypedef enum OrientationType { UPSIDEDOWN=-1, REGULAR=1 };
typedef enum MotorNdxType { FIRSTMOTOR=0, SECONDMOTOR };
typedef struct xstruct_motor {
int port;
OrientationType orientation;
int power;
long error;
long lastEncoderValue;
int stallCounter;
bool stalled;
bool moving;
int speed; // Current speed of the motor
} struct_motor;
typedef struct xstruct_motors {
struct_motor motorNum[2];
int powerLevel; // Default power used to set the power level for subsequent commands
int powerLevelRev;
int tempPowerLevel; // Power level of the current command executed. May be different from the default if the user specified a power level for a specific function
OrientationType orientation;
int direction; // Initial direction of the robot is set to Stopped
int lastDirection; // Last movement direction of the robot
int master;
int slave;
bool synched;
int turnRatio; // values range from -100 to +100
int count; // Count of the number of configured motors
} struct_motors;
struct_motors driveMotors;
struct_motors accessoryMotors;
bool IsFollowingLine = false; // Some of the Stop functions will need to know if the robot is following a line or not.
int colorSensor;
int touchSensorB;
//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};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
//
// 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);
}
void waitForEnterButton() {
display(6,"*Press Enter"); // Notify User
display(7,"* to continue. *");
while(nNxtButtonPressed!=kEnterButton)
{ // 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(thisMotor==motorA) { return 1; }
if(thisMotor==motorB) { return 2; }
if(thisMotor==motorC) { return 3; }
return 0;
}Motor Encoder
//////////////////////////////
// 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();
}
}Configuring the Robot Dimensions
//////////////////////////////////////////////////////////////////////
// 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.
void configureWheelCircumferenceMM() {
config.wheelCircumferenceMM = config.wheelDiameterMM * PI; // Formula. NOT changeable.
}Variables for Working with Lines
////////////////////////////////////////////////////////////////////////////////////
//
// 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;
//
Methods for Working with Lines
//
// 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]; }
// 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;
}Working With Light Sensors
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.
//if((long) SensorRaw[lightSensorA] > tHighLight) { tHighLight = (long) SensorRaw[lightSensorA]; }
//if((long) SensorRaw[lightSensorA] < tLowLight) { tLowLight = (long) SensorRaw[lightSensorA]; }
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.
//if((long) SensorRaw[lightSensorB] > tHighLight2) { tHighLight2 = (long) SensorRaw[lightSensorB]; }
//if((long) SensorRaw[lightSensorB] < tLowLight2) { tLowLight2 = (long) SensorRaw[lightSensorB]; }
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);
}
tHighLight = 0;
tCurrLight = 0;
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;
SensorType[S3] = sensorLightInactive;
}// This function retrieves the light range data for the two light sensors
void initLightSensor() {
}
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 * config.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 / config.wheelCircumferenceMM;
float wheelDegreesOfTurningNeeded = wheelRotationsPerRobotTurn * turnDegrees;
float targetDegrees = wheelDegreesOfTurningNeeded * config.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 / config.wheelCircumferenceMM;
float wheelDegreesOfTurningNeeded = wheelRotationsPerRobotTurn * turnDegrees;
float targetDegrees = wheelDegreesOfTurningNeeded * config.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 / config.wheelCircumferenceMM;
float wheelDegreesOfTurningNeeded = wheelRotationsPerRobotTurn * turnDegrees;
float targetDegrees = wheelDegreesOfTurningNeeded * config.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 / config.wheelCircumferenceMM;
float wheelDegreesOfTurningNeeded = wheelRotationsPerRobotTurn * turnDegrees;
float targetDegrees = wheelDegreesOfTurningNeeded * config.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 / config.wheelCircumferenceMM;
float wheelDegreesOfTurningNeeded = wheelRotationsPerRobotTurn * turnDegrees;
float targetDegrees = wheelDegreesOfTurningNeeded * config.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 / config.wheelCircumferenceMM;
float wheelDegreesOfTurningNeeded = wheelRotationsPerRobotTurn * turnDegrees;
float targetDegrees = wheelDegreesOfTurningNeeded * config.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();
}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.
}