RoboCatz.com

Spring 2015 Program Code in RobotC

#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 Parameters

typedef 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};

//////////////////////////////////////////////////////////////////////
//
// 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;
}

//////////////////////////////////////////////////////////////////////
//    M O T O R    E N C O D E R
// The MotorEncoder is a variable which keeps track of the number of degrees the motor has turned.
// This value is not purged until the robot is turned off.  This means that you can re-start a
// program and the MotorEncoder just keeps on counting.  If the program checks for a certain value
// (turning or going straight) and the encoder value cycles back to 0, then the program may result
// in a logic error.  To prevent this from happening, disallow any target values beyond the operating
// range.  If this results in a problem, insert resetRotationSensor() functions within the program.
void checkForEncoderError(int newValue) {
	if(newValue > 32000 || newValue < -32000) {
 		nxtDisplayCenteredTextLine(1, "ERROR: Over Max");
 		nxtDisplayCenteredTextLine(2, "for nMotorEncoder");
 		nxtDisplayCenteredTextLine(3, "Power down Robot.");
 		nxtDisplayCenteredTextLine(4, "Reset the encoder");
 		nxtDisplayCenteredTextLine(4, "in your program.");
	  waitForOrangeButton();
	}
}

//////////////////////////////////////////////////////////////////////
// Variables used to Configure Robot Dimensions
// Robot dimensions are stored in a text file on the NXT brick.  This allows for functions to be
// developed independent of the physical characteristics of the Robot.  Thus, different robots
// could use the same code as long as each robot's physical dimensions are stored in this file.
// Define internal constants and temporary handles for the file processes.
const string sFileNameRD = "RobotDimensions.txt";
TFileIOResult nIoResult;
TFileHandle hFileHandle;
int defaultFileSize = 40;  //  Only need space to store 4 Float values.  Each Float value uses 4 bytes.
int nFileSize = defaultFileSize;  //  Only need space to store 4 Float values.  Each Float value uses 4 bytes.

// There are 4 characteristics of the robot which are used in various move functions.
// Each of the size characteristics are entered in milimeters (MM).
// The four lines below are used only to declare these variables as Global.
// Do not enter any values for these variables here.  The values to be used by the program are read
// from the text file (mentioned earlier).  The file is read in the InitMove() function below.
float centerOfWheelToCenterOfRobotMM = 0;  // Distance of each drive wheel to the center of the robot
                                           // Be careful with tankbots.  This distance is to the sprockets, not the center of the tread.
void configureWheelCircumferenceMM() {
	config.wheelCircumferenceMM = config.wheelDiameterMM * PI;  // Formula.  NOT changeable.
}

void InitMove() {
  SensorType[touchSensor] = sensorTouch;   // Set 'touchSensor' to be of type sensorTouch
  SensorType[sonarSensor] = sensorSONAR;   // Set 'sonarSensor' to be of type sensorSONAR
//  SensorType[lightSensorA] = sensorLightInactive;
//  SensorType[lightSensorB] = sensorLightInactive;
  SensorType[colorSensor] = sensorLightInactive;
}
// The configureWheelToCenterOfRobotMM() function is generally not used since the configureRobotDimensions() function performs most work
void configureWheelToCenterOfRobotMM(float WheelToCenterOfRobot) {
  centerOfWheelToCenterOfRobotMM = WheelToCenterOfRobot;
}
void configureWheelDiameterMM(float WheelDiameter) {
  config.wheelDiameterMM = WheelDiameter;
  config.wheelCircumferenceMM = config.wheelDiameterMM * PI;  // Formula.  NOT changeable.
}

////////////////////////////////////////////////////////////////////////////////////
//
// 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;
//
//
// 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;
}
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;    // Go ahead and turn the light 'off'
  SensorType[S3] = sensorLightInactive;    // Go ahead and turn the light 'off'
}

// This function retrieves the light range data for the two light sensors
void initLightSensor() {
}

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

//////////////////////////////////////////////////////////////////////
//
// Various Stop Functions
//
void powerZero() {
  motor[motorA] = 0;                       // turn the motor off.
  motor[motorB] = 0;                       // turn the motor off.
  motor[motorC] = 0;                       // turn the motor off.
}
void stopMoving() {
  if(keepGoing) return;
  bFloatDuringInactiveMotorPWM = false;    // brake
  nSyncedMotors = synchNone;               // De-sync the motors.
  powerZero();
  direction = STOPPED;
  IsFollowingLine = false;                 // Stopping means you are not following any line
  if(SensorType[S2] == sensorLightActive)   { SensorType[S2] = sensorLightInactive; } // Turn it off
  if(SensorType[S3] == sensorLightActive)   { SensorType[S3] = sensorLightInactive; } // Turn it off

}

void loopUntilLine(int tempLightLevel) {
  if(direction == STOPPED) beginDrivingForward(powerLevel);
  if(direction == FORWARD) {
    SensorType[S3] = sensorLightActive;    // Make sure you turn the light 'on'
  } else {
    SensorType[S2] = sensorLightActive;    // Make sure you turn the light 'on'
  }
  // Enter a loop and stay there as long as the light value is greater than % in variable lineIsAtLevel
  display(6," Until Line     *");          // Clear the message
  display(7,"                *");
  while(getLightPercent() > tempLightLevel) {
    nxtDisplayTextLine(7, "LightLvl: %d", getLightPercent());  // Just a debug message
  }
  stopMoving();                            // Stop moving now
  if(SensorType[S3]==sensorLightActive) { SensorType[S3] = sensorLightInactive; }   // Go ahead and turn the light 'off'
  if(SensorType[S2]==sensorLightActive) { SensorType[S2] = sensorLightInactive; }   // Go ahead and turn the light 'off'
  addToDistance = false;
}
void untilLine(int tempLightLevel) {
	loopUntilLine(tempLightLevel);
}
void untilLine() {
	loopUntilLine(lineIsAtLevel);
}
void stopOnLine() { untilLine(); stopMoving(); }
void stopOnLine(int tempLightLevel) { untilLine(tempLightLevel); stopMoving(); }

void untilSonarLessThan(float distance_in_inches) {
  if(distance_in_inches <=3) {
    display(0,"*****************");
    display(2,"*!!  Warning  !!*");
    display(3,"*Sonar Sensor not");
    display(4,"*able to detect *");
    display(5,"*distance <4 inch");
    display(6,"*Press Orange Btn");
    display(7,"*to continue.   *");
	  waitForOrangeButton();
  }
  distance_in_inches = distance_in_inches + (float) ((float)powerLevel / 100 * 5.0);  // Add distance based on power level because it will take a few milliseconds before the robot is able to respondto the next instruction
  display(3," SONAR Checking *");          // Clear the message
  display(4," <<<<      >>>> *");
	float distance_in_cm; distance_in_cm = distance_in_inches * 2.54;
	nxtDisplayTextLine(5, "Crit %2d %2d", distance_in_inches, (int) distance_in_cm);
  nxtDisplayTextLine(6, "Curr  %d  ", SensorValue[sonarSensor]);
  while(SensorValue[(short) sonarSensor] > distance_in_cm) {
	  nxtDisplayTextLine(7, "Curr  %d  ", SensorValue[sonarSensor]);
    if(direction == STOPPED) beginDrivingForward(powerLevel);
    if(IsFollowingLine) { FollowLinePID(); }
  }
  addToDistance = false;
}
bool isSonarLessThan(float distance_in_inches) {
  float distance_in_cm; distance_in_cm = distance_in_inches * 2.54;
  return (SensorValue((short) sonarSensor) < distance_in_cm);
}
bool isSonarGreaterThan(float distance_in_inches) {
  float distance_in_cm; distance_in_cm = distance_in_inches * 2.54;
  return (SensorValue((short) sonarSensor) > distance_in_cm);
}

void stopIfSonarLessThan(float distance_in_inches) { untilSonarLessThan(distance_in_inches); stopMoving(); }

void untilSonarGreaterThan(float distance_in_inches) {
  float distance_in_cm; distance_in_cm = distance_in_inches * 2.54;
  while(SensorValue(sonarSensor) <= distance_in_cm) {
    if(IsFollowingLine) { FollowLinePID(); }  // If going forward, then check rotation sensor
  }
  addToDistance = false;
}

void untilDistance(float inches) {
  long rotationSensor = 0;
  int currPowerLevel= motor[mMotor];
  float targetDegrees;
  if(addToDistance) { inches+=1; addToDistance=false; }
  rotationSensor = nMotorEncoder[mMotor];
  nxtDisplayCenteredTextLine(5," Until Dis: %3.3f", config.gearRatio);          // Clear the message
  display(7,"                *");
                                           // config.wheelCircumferenceMM is in millimeters, so, multiply inches by 25.4 to convert to mm.
  targetDegrees = (float) 360 * (float) inches * 25.4 / (float) config.wheelCircumferenceMM * (float) config.gearRatio;  // Num of Degrees to turn
  bFloatDuringInactiveMotorPWM = false;    // brake
  targetDegrees *= direction * mDir;
  targetDegrees += rotationSensor;         // Add these degrees to the current rotation sensor
  checkForEncoderError(targetDegrees);     // Setting a MotorEncoder target to an invalid number may result in a logic error.  Better check first.
  nMotorEncoderTarget[mMotor] = targetDegrees; // This is the new target degree to achieve
  if(direction == STOPPED) beginDrivingForward(powerLevel);
//  if(direction * mDir == FORWARD) { PlaySoundFile("Forward.rso");PlaySoundFile("Turn.rso"); }
//  if(direction * mDir == BACKWARD) { PlaySoundFile("Backwards.rso"); }
  nxtDisplayCenteredTextLine(6, "Encoder: %d", nMotorEncoder[mMotor]);
  nxtDisplayCenteredTextLine(7, " Target: %d", targetDegrees);
  //while(nMotorRunState[mMotor] != runStateIdle && nMotorRunState[mMotor] != runStateHoldPosition && ((direction*mDir==FORWARD && nMotorEncoder[mMotor] <= targetDegrees) || (direction*mDir==BACKWARD && nMotorEncoder[mMotor] >= targetDegrees))) {
  while(((direction*mDir==FORWARD && nMotorEncoder[mMotor] <= targetDegrees) || (direction*mDir==BACKWARD && nMotorEncoder[mMotor] >= targetDegrees))) {
    motor[mMotor] = currPowerLevel;
  	nxtDisplayCenteredTextLine(2, "mMotorPwr: %d", motor[mMotor]);
  	nxtDisplayCenteredTextLine(3, "sMotorPwr: %d", motor[sMotor]);
    if(IsFollowingLine) { FollowLinePID(); }
  }

//  PlaySoundFile("Bravo.rso");
}
void stopAfterDistance(float inches) { untilDistance(inches); stopMoving(); }


void untilRotations(float turnRotations) {
  long rotationSensor = 0;
  float targetDegrees;
  rotationSensor = nMotorEncoder[motorB];
                                                    // config.wheelCircumferenceMM is in millimeters, so, multiply inches by 25.4 to convert to mm.
  targetDegrees = 360 * turnRotations;  // Num of Degrees to turn
  bFloatDuringInactiveMotorPWM = false;             // brake
  targetDegrees *= direction;
  targetDegrees += rotationSensor;                  // Add these degrees to the current rotation sensor
  checkForEncoderError(targetDegrees);              // Setting a MotorEncoder target to an invalid number may result in a logic error.  Better check first.
  nMotorEncoderTarget[motorB] = targetDegrees;      // This is the new target degree to achieve
  if(direction == BACKWARD) {                       // If going backward, then check rotation sensor >= target
    while(nMotorRunState[motorB] != runStateIdle && nMotorRunState[motorB] != runStateHoldPosition && nMotorEncoder[motorB] >= targetDegrees) {
      if(IsFollowingLine) { FollowLinePID(); }
    }
  } else {                                          // If going forkward, then check rotation sensor <= target
    while(nMotorRunState[motorB] != runStateIdle && nMotorRunState[motorB] != runStateHoldPosition && nMotorEncoder[motorB] <= targetDegrees) {
      if(IsFollowingLine) { FollowLinePID(); }
    }
  }
}
void stopAfterRotations(float inches) { untilRotations(inches); stopMoving(); }

void accelerateOverDistance(float inches, int power) {
  long rotationSensor = 0;
  long rotationSensor2 = 0;
  long rotationSensorStart = 0;
  long rotationSensorStart2 = 0;
  float targetDegrees;
  if(direction == STOPPED) beginDrivingForward(18);
  if(direction == FORWARD) {                        // If going forward, then check rotation sensor <= target
     motor[motorB] = 18;              // Starting Power Level
  } else {
     motor[motorB] = -18;              // Starting Power Level
  }
  float powerIncrement = (power-20) / 10;
  float incrementAtDegree;
  rotationSensor = nMotorEncoder[motorB];
                                                    // config.wheelCircumferenceMM is in millimeters, so, multiply inches by 25.4 to convert to mm.
  targetDegrees = 360 * inches * 25.4 / config.wheelCircumferenceMM * config.gearRatio;  // Num of Degrees to turn
  bFloatDuringInactiveMotorPWM = false;             // brake
  if(direction == BACKWARD) { targetDegrees*= -1; } // Direction is reverse if <0
  targetDegrees += rotationSensor;                  // Add these degrees to the current rotation sensor
  checkForEncoderError(targetDegrees);              // Setting a MotorEncoder to an invalid number may result in an error.  Better check first.
  incrementAtDegree = rotationSensor;
  nMotorEncoderTarget[motorB] = targetDegrees;      // This is the new target degree to achieve
  if(direction == FORWARD) {                        // If going forward, then check rotation sensor <= target
    while(nMotorRunState[motorB] != runStateIdle && nMotorRunState[motorB] != runStateHoldPosition && nMotorEncoder[motorB] <= targetDegrees) {
      if(nMotorEncoder[motorB] > incrementAtDegree) {	incrementAtDegree+=36; motor[motorB] += powerIncrement; }
      if(IsFollowingLine) { FollowLinePID(); }
    }
  } else {                                           // If going backward, then check rotation sensor >= target
    while(nMotorRunState[motorB] != runStateIdle && nMotorRunState[motorB] != runStateHoldPosition && nMotorEncoder[motorB] >= targetDegrees) {
      if(nMotorEncoder[motorB] < incrementAtDegree) { incrementAtDegree-=36; motor[motorB] -= powerIncrement; }
      if(IsFollowingLine) { FollowLinePID(); }
    }
  }
}
void stopAfterDistanceWithAcceleration(float inches, int power) { accelerateOverDistance(inches, power); stopMoving(); }

void untilTouch() {
  while(SensorValue(touchSensor) == 0) {   // Loop until the button is pressed
    if(direction == FORWARD) {             // If going forward, then check rotation sensor <= target
      if(IsFollowingLine) { FollowLinePID(); }
    } else {                                // If going backward, then check rotation sensor >= target
      if(IsFollowingLine) { FollowLinePID(); }
    }
  }
}
void stopOnTouch() { untilTouch(); stopMoving(); }
void untilRelease() {
  while(SensorValue(touchSensor) == 1) {   // Loop until the button is pressed
  }
}

//////////////////////////////////////////////////////////////////////
//
// Special Functions
//

float rotations(float tempRotations) {     // Shortcut to convert Rotations into degrees
  return tempRotations * 360;              // May be useful when you want to specify a certain number of
}                                          // rotations and the other function requires degrees
                                           // For example: accessoryDown(rotations(5)); moves accessory for 5 rotations

void forwardToLine() {
	beginDrivingForward(powerLevel/2);
  SensorType[S3] = sensorLightActive;    // Make sure you turn the light 'on'
  SensorType[S2] = sensorLightActive;    // Make sure you turn the light 'on'
  while(getLightPercentA() > lineIsAtLevel && getLightPercentB() > lineIsAtLevel) { }
  if(getLightPercentA() > lineIsAtLevel) {
    nSyncedMotors = synchBC;               // No motor synch
    bFloatDuringInactiveMotorPWM = false;    // Enable braking
    nSyncedTurnRatio = -100;                 // motors move in opposite directions of one another
    motor[motorB]=powerLevelRev/2;
    while(getLightPercentA() > lineIsAtLevel) {   // Loop until the button is pressed
    }
    stopMoving();
  } else {
	  if(getLightPercentB() > lineIsAtLevel) {
  	  nSyncedMotors = synchCB;               // No motor synch
	    bFloatDuringInactiveMotorPWM = false;    // Enable braking
      nSyncedTurnRatio = -100;                 // motors move in opposite directions of one another
    	motor[motorC]=powerLevelRev/2;
  	  while(getLightPercentB() > lineIsAtLevel) {   // Loop until the button is pressed
	    }
    	stopMoving();
  	}
  }
  SensorType[S3] = sensorLightInactive;    // Make sure you turn the light 'off'
  SensorType[S2] = sensorLightInactive;    // Make sure you turn the light 'off'
  stopMoving();                            // Stop moving now
  resetRotationSensor(motorB, motorC);
}

void beginBackIntoWall(int tempPower) {
  beginDrivingBackward(tempPower);
  while(SensorValue(touchSensor) == 0 && SensorValue(touchSensorB) == 0) {   // Loop until the button is pressed
    if(direction == FORWARD) {             // If going forward, then check rotation sensor <= target
      if(IsFollowingLine) { FollowLinePID(); }
    } else {                                // If going backward, then check rotation sensor >= target
      if(IsFollowingLine) { FollowLinePID(); }
    }
  }
  if(SensorValue(touchSensorB) == 0) {
    while(SensorValue(touchSensorB) == 0) {   // Loop until the button is pressed
      nSyncedMotors = synchNone;               // No motor synch
      bFloatDuringInactiveMotorPWM = false;    // Enable braking
      if(direction == BACKWARD) {             // If going forward, then check rotation sensor <= target
        motor[motorC]=tempPower;
	    } else {
        motor[motorC]=tempPower * -1;
  		}
    }
    stopMoving();
  }
  if(SensorValue(touchSensor) == 0) {
    while(SensorValue(touchSensor) == 0) {   // Loop until the button is pressed
      nSyncedMotors = synchNone;               // No motor synch
      bFloatDuringInactiveMotorPWM = false;    // Enable braking
      if(direction == BACKWARD) {             // If going forward, then check rotation sensor <= target
        motor[motorB]=tempPower;
	    } else {
        motor[motorB]=tempPower * -1;
  		}
    }
    stopMoving();
  }
  beginDrivingBackward(tempPower);
  stopAfterDistance(0.20);
  resetRotationSensor(motorB, motorC);
}
void backIntoWall() {
	beginBackIntoWall(powerLevel);
}
void backIntoWall(int tempPower) {
	beginBackIntoWall(tempPower);
}


/////////////////////////////////////////////////////////////////////////////////////////////
//
// Define the Overloaded Functions
//
void pause() {
  waitForTouchSensor();
}
void driveForward() {                      // Overloaded function: 0 parameters
  beginDrivingForward(powerLevel);
}
void driveForward(float inches) {          // Overloaded function: 1 parameter
  beginDrivingForward(powerLevel);
  stopAfterDistance(inches);
}
void driveForward(float inches, int power) {   // Overloaded function: 2 parameters
  beginDrivingForward(power);
  stopAfterDistance(inches);
}
void driveBackward() {                     // Overloaded function: 0 parameters
  beginDrivingBackward(powerLevel);
}
void driveBackward(float inches) {         // Overloaded function: 1 parameter
  beginDrivingBackward(powerLevel);
  stopAfterDistance(inches);
}
void driveBackward(float inches, int power) {  // Overloaded function: 2 parameters
  beginDrivingBackward(power);
  stopAfterDistance(inches);
}
void followLine(int thisSide, float inches) {
  followLine(thisSide);
  stopAfterDistance(inches);
}
void followWall(int thisSide) {             // Overloaded function: 1 parameters
  beginFollowingWall(thisSide, powerLevel);
}
void followWall(int thisSide, float inches) {  // Overloaded function: 2 parameters
  beginFollowingWall(thisSide, powerLevel);
  stopAfterDistance(inches);
}
void followWall(int thisSide, float inches, int power) {  // Overloaded function: 3 parameters
  beginFollowingWall(thisSide, power);
  stopAfterDistance(inches);
}

void wait() { wait(1); }  // Overload the Wait function to create a default without parameters

                                               // Overloaded function: 2 parameters
void gradualTurnClockwise(float turnDegrees, float turnRadiusIN) {
  gradualTurnClockwise(turnDegrees, turnRadiusIN, powerLevel);
}
void gradualTurnCounterClockwise(float turnDegrees, float turnRadiusIN) {
  gradualTurnCounterClockwise(turnDegrees, turnRadiusIN, powerLevel);
}

void swingTurnClockwise(float turnDegrees) {   // Overloaded function: 1 parameter
  swingTurnClockwise(turnDegrees, powerLevel);
}
void swingTurnCounterClockwise(float turnDegrees) {// Overloaded function: 1 parameter
  swingTurnCounterClockwise(turnDegrees, powerLevel);
}
void swingTurn(int dirRotation, float turnDegrees, int tempPower) { // new function: swingTurn()
  if(dirRotation == COUNTERCLOCKWISE) {
    swingTurnCounterClockwise(turnDegrees, tempPower);
  } else {
    swingTurnClockwise(turnDegrees, tempPower);
  }
}
void swingTurn(int dirRotation, float turnDegrees) { // Overloaded function: 2 parameters
  swingTurn(dirRotation, turnDegrees, powerLevel);
}
void pointTurnClockwise(float turnDegrees) { pointTurnClockwise(turnDegrees, powerLevel); }
void pointTurnCounterClockwise(float turnDegrees) { pointTurnCounterClockwise(turnDegrees, powerLevel); }
void pointTurn(int dirRotation, float turnDegrees, int tempPower) {   // new function: pointTurn()
  if(dirRotation == COUNTERCLOCKWISE) {
    pointTurnCounterClockwise(turnDegrees, tempPower);
  } else {
    pointTurnClockwise(turnDegrees, tempPower);
  }
}
void pointTurn(int dirRotation, float turnDegrees) {  // Overloaded function: 2 parameters
  pointTurn(dirRotation, turnDegrees, powerLevel);
}


// This is a generic function to run both initialization functions.
// The initialization functions could be run individually for smaller programs.
// This Startup block must be included at the end of the file so that the
// references to functions shown below do not generate compiler errors which would happen
// if the function is referenced before it is actually defined.  Functions are defined first.  Then referenced.
void startup() {
  //sFileNameRD = "RobotDim.txt";

  InitMove();                              // Loads the physical characteristics of the robot
  initLightSensor();                       // Loads the light sensor ranges to be used for identifying and following lines
  resetRotationSensor(motorA, motorB, motorC);
  config.wheelCircumferenceMM = config.wheelDiameterMM * PI;  // Formula.  NOT changeable.
  if(1==2) {                               // This block is NOT executed.  It is included here only so that warnings
                                           // regarding Unreferenced Functions are not shown.
    display(0,"");
    wait(0);
    startup();
    waitForOrangeButton();
    setPowerLevel(0); setDefaultSpeed(0); setAccessoryPowerLevel(0);
    resetRotationSensor(motorA);
    getLightDiffPortion();
    getLightPercentA();
    getLightPercentB();
    accelerateOverDistance(0,0);
    driveBackward();
    driveBackward(0);
    driveBackward(0,0);
    driveForward();
    driveForward(0);
    driveForward(0,0);
    followWall(0);
    followWall(0,0);
    followWall(0,0,0);
    backIntoWall();
    Motor(0,0,0);
    gradualTurnCounterClockwise(0,0);
    gradualTurnClockwise(0,0);
    swingTurnCounterClockwise(0);
    swingTurnClockwise(0);
    swingTurn(CLOCKWISE,90);
    stopOnLine();
    untilDistance(0);
    stopAfterRotations(0);
    stopAfterDistance(0);
    stopOnTouch();
    stopIfSonarLessThan(0);
    isSonarLessThan(0);
    isSonarGreaterThan(0);
    stopAfterDistanceWithAcceleration(0,0);
    untilTouch();
    untilRelease();
    untilSonarGreaterThan(0);
    waitForTouchSensor();
    pause();
    followLine(LEFTSIDE);
    calibrateLightSensor();
    createLightCalibrationFile(0);
    configureWheelToCenterOfRobotMM(centerOfWheelToCenterOfRobotMM);
//    configureWheelDiameterMM(wheelDiameterMM);
//    ConfigureRobotDimensions(centerOfWheelToCenterOfRobotMM,wheelDiameterMM,gearRatio,FORWARD);
    sideOfLine = MIDDLE;                   // Initialized here only to avoid a compiler warning that they are not used anywhere else.
    sideOfLine = RIGHTSIDE;
    sideOfLine = LEFTSIDE;
  }
}