•
•
•
•
RoboCatz.com

Our Function Library (Updated Fall 2017)


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.

The "genericRobotStructure" shown below is a primitive form of object oriented programming. This structure will create a 'robot' object of type 'robotObj' that will have several properties including:
Other functions will then use the robot object to get the properties needed to determine how to move the robot. One important thing to remember...You must initialize these properties before they can be used. This initialization usually takes place early in the main task. 

//////////////////////////////////////////////////////////////////////
//
// Define constants and variables to be used in the functions below
//

typedef struct genericRobotStruct {
  int powerLevel;
  int wheelDiameterMM;
  int gearRatio;
  int distanceBetweenWheelsMM;
  bool criteriaNotMetYet;
} robotObj; robotObj robot;

#define PI 3.14159
// There are 4 properties of the robot which are used in various move functions.
// Each of the size properties are entered in milimeters (MM).
                                           // Be careful with tankbots.  This distance is to the sprockets, not the center of the tread.
// wheelDiameterMM = 43.2;              // Self explanatory. Just look at the tire to see the diameter in MM.  Use a default of 43.2--that's the typical tire we use

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

#define setPowerLevel(pPowerLevel) robot.powerLevel=pPowerLevel

Moving Straight Functions Defined

void driveForward() {
  setMotorSync(B, C, 0, robot.powerLevel);
}
void driveForwardOnWall() {
  setMotorSync(C, B, 15, robot.powerLevel);
  resetMotorEncoder(B); resetMotorEncoder(C);
}
void driveBackward() {
  setMotorSync(B, C, 0, robot.powerLevel * -1);
}
void driveBackwardOnWall() {
  setMotorSync(C, B, 5, robot.powerLevel * -1);
  resetMotorEncoder(B); resetMotorEncoder(C);
}

Turning Functions Defined

//////////////////////////////////////////////////////////////////////
// Just a simple swing-turn
void turnRight(int degrees){
  resetMotorEncoder(C);
  long startingEncoderValue = getMotorEncoder(C);
  motor[motorC]=20;   // Rotate the wheel forwards
  while(abs(getMotorEncoder(C)-startingEncoderValue) < degrees*12) {
  }
  stopAllMotors();
}

void turnLeft(int degrees){
  resetMotorEncoder(B);
  long startingEncoderValue = getMotorEncoder(B);
  motor[motorB]=20;   // Rotate the wheel forwards
  while(abs(getMotorEncoder(B)-startingEncoderValue) < degrees*3/5) {
  }
  stopAllMotors();
}
void pointTurnRight(int degrees){
	resetMotorEncoder(B);
	long startingEncoderValue = getMotorEncoder(B);
	motor[motorB]=-20;   // Rotate the wheel forwards
	motor[motorC]=20;   // Rotate the wheel forwards
	while(abs(getMotorEncoder(B)-startingEncoderValue) < degrees*6) {
	}
	stopAllMotors();
}
void pointTurnLeft(int degrees){
	resetMotorEncoder(B);
	long startingEncoderValue = getMotorEncoder(B);
	motor[motorB]=20;   // Rotate the wheel forwards
	motor[motorC]=-20;   // Rotate the wheel forwards
	while(abs(getMotorEncoder(B)-startingEncoderValue) < degrees*6) {
	}
	stopAllMotors();
}
void gradualTurnClockwise(float degrees, int turnRadiusIN) {
  if(!OkToMove) return;
  degrees -= 16 / (360 / degrees);
  float innerTurnRadius = ((turnRadiusIN * 25.4) - (robot.distanceBetweenWheelsMM/2)) * 2 * PI * (degrees/360);
  float outerTurnRadius = ((turnRadiusIN * 25.4) + (robot.distanceBetweenWheelsMM/2)) * 2 * PI * (degrees/360);
  float turnRatio = 50 - (innerTurnRadius/outerTurnRadius * 50);
  setMotorSync(C, B,-1 * turnRatio,20);
  resetMotorEncoder(B); resetMotorEncoder(C);
  repeatUntil(getMotorEncoder(C)>(outerTurnRadius/(robot.wheelDiameterMM*PI))*360) { sleep(5);	}
  setMotorSpeeds(0,0);
  stopAllMotors();
}
void gradualTurnCounterClockwise(float degrees, int turnRadiusIN) {
  if(!OkToMove) return;
  degrees -= 16 / (360 / degrees);
  float innerTurnRadius = ((turnRadiusIN * 25.4) - (robot.distanceBetweenWheelsMM/2)) * 2 * PI * (degrees/360);
  float outerTurnRadius = ((turnRadiusIN * 25.4) + (robot.distanceBetweenWheelsMM/2)) * 2 * PI * (degrees/360);
  float turnRatio = 50 - (innerTurnRadius/outerTurnRadius * 50);
  setMotorSync(B, C, turnRatio,20);
  resetMotorEncoder(B); resetMotorEncoder(C);
  repeatUntil(getMotorEncoder(B)>(outerTurnRadius/(robot.wheelDiameterMM*PI))*360) { sleep(5);	}
  setMotorSpeeds(0,0);
  stopAllMotors();
}
void gradualTurnClockwise(float degrees) { gradualTurnClockwise(degrees, 20); }
void gradualTurnCounterClockwise(float degrees) { gradualTurnCounterClockwise(degrees, 20); }

"Until" Functions

////////////////////////////////////////////////////////////////////////
//
void untilDistance(int moveIN) {
  resetMotorEncoder(B);
  resetMotorEncoder(C);
  float targetDegree;
  targetDegree=(moveIN * 25.4)/(robot.wheelDiameterMM*PI)*360;
  repeatUntil(abs(getMotorEncoder(B)) > targetDegree) { sleep(5);	}
}
void untilTouch() {
  while(SensorValue(touchSensor) == 0) {   // Loop until the button is pressed
    sleep(5);
  }
}
void untilSonarLessThan(float distance_in_inches) {
  float distance_in_cm; distance_in_cm = distance_in_inches * 2.54;
  while(SensorValue((short) ultrasonicSensor) > distance_in_cm) {
  	displayCenteredBigTextLine(1, "Sensor %d", SensorValue[ultrasonicSensor]);
   sleep(5);
  }
}
void untilArmTravelled(int degrees, int defPower) {
  resetMotorEncoder(A);      // Reset the motor
  float targetDegree;
  targetDegree=(degrees * 5);// the small tan gears are 5 times smaller than the turntable gear
  if(degrees<0) { degrees *= -1; defPower *= -1; }  // If degrees are negative, then reverse the power
  motor[A] = defPower * -1;  // make the power reversed because of the gearbox being used
  repeatUntil(abs(getMotorEncoder(A)) > targetDegree) { sleep(5); }
  motor[A]=0;
}

Function Overloading

// Overloaded function
void driveForward(float distance) {
  driveForward();
  untilDistance(distance);
  stopAllMotors();
}
void driveForwardOnWall(float distance) {
  driveForwardOnWall();
  untilDistance(distance);
  stopAllMotors();
}
void driveBackward(float distance) {
  driveBackward();
  untilDistance(distance);
  stopAllMotors();
}
void driveBackwardOnWall(float distance) {
  driveBackwardOnWall();
  untilDistance(distance);
  stopAllMotors();
}

Follow Line Task

//////////////////////////////////////////////////////////////////////
//
task FollowLine_Edge () {

  int diff;

  while(true) {
    displayCenteredBigTextLine(1, "Sensor %d", SensorValue[colorSensor1]);

    int k=20;
    int defaultPower=20;
    diff = SensorValue[colorSensor1] - k;
    displayCenteredBigTextLine(1, "Sensor %d", SensorValue[colorSensor1]);
    displayCenteredBigTextLine(4, "diff %d", diff);
    diff/=3;
    motor[B]=defaultPower-diff;
    motor[C]=defaultPower+diff;
  }
}

//////////////////
// Follow Center of the line
//     Light sensor on each edge
task FollowLine () {
  int diff;
  while(true) {
    displayCenteredBigTextLine(1, "Sensor %d", SensorValue[colorSensor1]);
    int defaultPower=20;
    diff = SensorValue[colorSensor2] - SensorValue[colorSensor1];
    displayCenteredBigTextLine(1, "Sensor %d", SensorValue[colorSensor1]);
    displayCenteredBigTextLine(4, "diff %d", diff);
    diff/=6;
    motor[B]=defaultPower-diff;
    motor[C]=defaultPower+diff;
  }
}

Stopping on Line When Background is Complex

//////////////////////////////////////////////////////////////////////
//
void stopOnLine() {
  while(SensorValue[colorSensor1]>20) {
    sleep(5);
  }
  stopAllMotors();
}

int sensorReadings[20];
int sensorReadingNdx=0;
void stopOnLine() {
  for(sensorReadingNdx=0;sensorReadingNdx<20;sensorReadingNdx++) { sensorReadings[sensorReadingNdx]=SensorValue[colorSensor1]*1.0; }
  writeDebugStreamLine("%s", "This is a test");
  while(true) {
    int thisReading = SensorValue[colorSensor1]*1.0;
    if(thisReading > 0.00) {
      sensorReadingNdx++;
      sensorReadings[sensorReadingNdx % 20]=thisReading;
      writeDebugStreamLine("%3d",thisReading);
      if(sensorReadings[(sensorReadingNdx - 10) % 20] - thisReading > 30.0) {
        writeDebugStreamLine("%s", "Diff > 30");
        if(thisReading < 10.00 && thisReading > 0.00) {
          writeDebugStreamLine("%s", "Black Line Detected");
          writeDebugStreamLine("%3d",thisReading);
          break;
        }
      }
    }
    sleep(10);
  }
  stopAllMotors();
}

void stopOnLine1() {
  for(sensorReadingNdx=0;sensorReadingNdx<20;sensorReadingNdx++){sensorReadings[sensorReadingNdx]=SensorValue[colorSensor1]*1.0; }

  writeDebugStreamLine("%s", "This is a test");
  while(true) {
    int thisReading = SensorValue[colorSensor1]*1.0;
    if(thisReading>0 && thisReading<100){
      sensorReadings[sensorReadingNdx++ % 20]=thisReading;
      if(sensorReadings[(sensorReadingNdx - 10) % 20] - thisReading > 25.0) {
        writeDebugStreamLine("%3d",thisReading);
        break;
      }
      writeDebugStreamLine("%3d",thisReading);

    }
    sleep(10);
  }
  stopAllMotors();
}

Delivering Water to the Fountain

task main()
{
  robot.wheelDiameterMM = 43.2;
  robot.distanceBetweenWheelsMM = 130;
  robot.powerLevel=40;

  waitForButtonPress();
  //wait(8);
  driveForward(22);
  robot.powerLevel=60;
  driveBackward(70);

}

Task Main - Getting Hoop from the Ramp

task main()
{
  robot.wheelDiameterMM = 43.2;
  robot.distanceBetweenWheelsMM = 130;
  robot.powerLevel=(40);

  waitForButtonPress();

  driveBackward(1);
  robot.powerLevel=(60);
  driveForward(40);
  robot.powerLevel=(40);
  driveForward();
  stopOnLine1();
  stopAllMotors();
  driveForward(2);
  pointTurnLeft(88);

  driveBackward();
  untilTouch();
  stopAllMotors();
  driveForward(18);

  motor[D2]=40;
  wait(1.5);
  motor[D2]=0;


  driveBackward();
  untilTouch();

  resetMotorEncoder(B); resetMotorEncoder(C);
  setMotorSync(B, C, -40, robot.powerLevel);
  repeatUntil(getMotorEncoder(C)>1340) { sleep(5);  }
  setMotorSpeeds(0,0);
  stopAllMotors();
  driveForward();
  stopOnLine1();
  driveBackward(10);
  pointTurnLeft(90);

  motor[A]=20;
  wait(0.7);
  motor[A]=-20;
  wait(0.7);
  motor[A]=20;
  wait(0.7);
  motor[A]=-20;
  wait(0.7);
  motor[A]=0;

  driveForward(1);
  motor[A]=20;
  wait(0.7);
  motor[A]=-20;
  wait(0.7);
  motor[A]=20;
  wait(0.7);
  motor[A]=-20;
  wait(0.7);
  motor[A]=0;


  driveBackward();
  untilTouch();
  stopAllMotors();

  driveForward(10);
  pointTurnLeft(85);
  driveForward();
  stopOnLine1();
  robot.powerLevel=(100);
  driveForward(54);

}

Filter and Water Pump

task main()
{
  wait(1);
  robot.wheelDiameterMM =33;
  robot.distanceBetweenWheelsMM = 300.08;
  robot.powerLevel=50;

  // Step 1: Sorting Hat
  waitForButtonPress();

  driveBackward(1);  // Align with the Jig and Wall
  driveForward(21);  // Drive out of base
  pointTurnLeft(90); // Turn toward the Sorter
  driveForward(7.5);// Push lever in
  driveBackward(9);  // Drive far enough backward to clear the water treatment plant
  pointTurnRight(90);// back into the base
  robot.powerLevel=100;
  driveBackward();   // Drive backward
  untilTouch();      // Until you hit the Jig
  stopAllMotors();   // Then stop moving
  wait(1);


  // Step 2: Pump
  while(!getButtonPress(buttonEnter)) { // Wait Here
    sleep(5);
  }
  driveBackward(1);
  driveForward(12);
  pointTurnLeft(90);
  driveForward(16);
  robot.powerLevel=30;
  wait(1);
  pointTurnRight(12);
  driveForward(6);
  driveBackward(0.5);

  motor[A]=-20;
  wait(3);
  motor[A]=0;
  /*pointTurnRight(25);
  motor[A]=-20;
  wait(3);
  motor[A]=0;*/
  pointTurnLeft(25);
  motor[A]=-20;
  wait(3);
  motor[A]=0;
  /*pointTurnLeft(25);
  motor[A]=-20;
  wait(3);
  motor[A]=0;*/

  driveForward(1);
  pointTurnLeft(50);
  pointTurnRight(25);
  //driveForward(1);
  robot.powerLevel=70;
  driveBackward(24);
  pointTurnRight(90);
  robot.powerLevel=100;
  driveBackward();
  untilTouch();
  stopAllMotors();

}