Our Function Library (Updated Fall 2016)
A
function library is a set of functions, variables, constants, and algorithms that are
included with the basic RobotC programs. The functions and variables in the library can then be referenced and called upon as needed in your program.
The purpose of keeping these functions and variables in one file is to make your programs more modular. Many programs can refer to and use the same function library. Also, if these functions and variables are stored in a file separate from your program, then you do not have to include all of the code below in order to access it. You can simply use one line of code to tell the compiler to include the contents of the entire library into your program. The compilter will do that at the time your program is compiled.
Structure
Most function libraries have a structure to help make them easier to understand by programmers. The format of this structure is one where the constants and variables are declared (for the most part) at the beginning of the file. This is also somewhat necessary since a constant or variable cannot be used by any part of the program before it has been defined (declared).
Declaration of Constants and Variables
The statements below show the definitions of various constants and variables. Most are assigned a value in their declaration.
//////////////////////////////////////////////////////////////////////
//
// Define constants and variables to be used in the functions below
//
typedef struct genericRobotStruct {
int powerLevel;
int wheelDiameterMM;
int gearRatio;
int distanceBetweenWheelsMM;
bool criteriaNotMetYet;
} robotObj; robotObj robot;
#define PI 3.14159// There are 4 characteristics of the robot which are used in various move functions.
// Each of the size characteristics 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 usePrimitives
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 turnRight2(int degrees){
resetMotorEncoder(C);
long startingEncoderValue = getMotorEncoder(C);
motor[motorC]=20; // Rotate the wheel forwards
while(abs(getMotorEncoder(C)-startingEncoderValue) < degrees*3) {
}
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) {
}
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 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(int distance) {
driveForward();
untilDistance(distance);
stopAllMotors();
}
void driveForwardOnWall(int distance) {
driveForwardOnWall();
untilDistance(distance);
stopAllMotors();
}
void driveBackward(int distance) {
driveBackward();
untilDistance(distance);
stopAllMotors();
}
void driveBackwardOnWall(int 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;
}
}Detecting Lines and Colors
//////////////////////////////////////////////////////////////////////
//
void stopOnLine() {
while(SensorValue[colorSensor1]>20) {
sleep(5);
}
stopAllMotors();
}Task Main - the "Mastery"
This main program uses a menu where each item from the menu is selected by pressing one of the buttons. Each sub-program is assigned to a particular button. When you press a button, the sub-program assigned to that button will be executed.
task main()
{
int selectedItem = 0;
while(true) { // Start an infinite while loop
/*-------------------------------------
Start the Menu loop here
-------------------------------------*/
while(true) { // Wait Here
// Break out of the loop if a button was pressed
// Pressing a button sets a variable to be used in a switch statement
if (getButtonPress(buttonUp)) { selectedItem = 2; playSoundFile("Click"); break; }
if (getButtonPress(buttonRight)){ selectedItem = 4; playSoundFile("Click"); break; }
if (getButtonPress(buttonDown)) { selectedItem = 1; playSoundFile("Click"); break; }
if (getButtonPress(buttonLeft)){ selectedItem = 3; playSoundFile("Click"); break; }
if (getButtonPress(buttonEnter)){ selectedItem = 5; playSoundFile("Click"); break; }
}
/*-------------------------------------
Now select the mission program based on the button pressed
-------------------------------------*/
switch (selectedItem)
{
/*-------------------------------------*/
case 1: //sharktank mission
setLEDColor(ledRedPulse);
// untilArmTravelled(90, -20);
wait(1);
robot.wheelDiameterMM = 56;
robot.powerLevel=10;
//driveBackward(3);
driveForward(36);
//wait(7);
stopAllMotors();
//driveForward(36);
driveBackward(50);
wait(2);
//untilTouch();
stopAllMotors(); // Stop now.
break;/*-------------------------------------*/
case 2: //get the bee mission
setLEDColor(ledGreenPulse);
wait(1);
robot.wheelDiameterMM = 200;
robot.powerLevel=50;
motor[motorA]=10; // Reset the position of the arms
motor[motorD]=-10; // Reset the position of the arms
wait(1);
motor[motorD]=0; // Arm is steep at this point
motor[motorA]=0; // Arm is steep at this point
wait(1);
driveForwardOnWall(114);
motor[motorD]=10; // Reset the position of the arms
wait(1);
motor[motorD]=0; // Arm is steep at this point
wait(1);
driveBackward(420);
wait(2);
//motor[motorA]=-10; // Reset the position of the arms
//motor[motorD]=-10;
//wait(1);
motor[motorD]=0; // Arm is steep at this point
setMotorSyncEncoder(A, D2, 0, 30, -15); // Move the arm 20 degrees at 15% downward power
wait(2);
//untilTouch();
stopAllMotors(); // Stop now.
break;
/*--------------------------------------*/
case 3: // the "helping" dog mission
setLEDColor(ledOrangePulse);
wait(1);
robot.distanceBetweenWheelsMM = 120;
robot.wheelDiameterMM = 56;
robot.powerLevel=40;
driveBackward(3);
driveForward(21);
gradualTurnClockwise(93, 5);
robot.powerLevel=30;
driveForward(22);
robot.powerLevel=20;
gradualTurnClockwise(160, 7);
driveForward(36);
wait(2);
stopAllMotors();
wait(2);
//untilTouch();
break;
/*--------------------------------------*/
case 4: //levi deliver the bee
setLEDColor(ledRedFlash);
wait(1);
robot.wheelDiameterMM = 200;
driveForward(110);
driveBackward(110);
//stopAllMotors();
break;
/*-------------------------------------*/
case 5: //levis new amazing program: delivers the bee, pig, and retrieves the seal's camera
setLEDColor(ledOrangeFlash);
wait(1);
robot.powerLevel=30;
robot.wheelDiameterMM = 200;
motor[A]=30;
wait(1);
motor[A]=0;
motor[D2]=-30;
wait(3);
motor[D2]=0;
driveForwardOnWall(/*112*/128);
motor[A]=-10;
resetMotorEncoder(A);
while(abs(nMotorEncoder[A])<510){
}
motor[A]=0;
wait(1); // Drop off the pig
motor[A]=30; // Bring arm back to the robot
resetMotorEncoder(A);
while(abs(nMotorEncoder[A])<450){
}
wait(2);
motor[A]=0;
wait(1);
robot.powerLevel=20; // Slow down when delivering the bee
driveForwardOnWall(/*78*/90); // toward seal
motor[D2]=10; // Get the camera hook ready
wait(2);
motor[D2]=0;
robot.powerLevel=30;
driveForwardOnWall(/*35*/40); // Hook the camera
motor[D2]=-10; // Raise the camera
wait(2);
motor[D2]=0;
robot.powerLevel=40;
driveBackwardOnWall(/*240*/ 400); // Return to base
motor[motorA]=0;
wait(2);
stopAllMotors();
break;
}
}
}