Our Function Library (Updated Fall 2015)
A
function library is a set of functions, variables, constants, and algorithms that are
included with the basic RobotC programs. The functions and variables in the library can then be referenced and called upon as needed in your program.
The purpose of keeping these functions and variables in one file is to make your programs more modular. Many programs can refer to and use the same function library. Also, if these functions and variables are stored in a file separate from your program, then you do not have to include all of the code below in order to access it. You can simply use one line of code to tell the compiler to include the contents of the entire library into your program. The compilter will do that at the time your program is compiled.
Structure
Most function libraries have a structure to help make them easier to understand by programmers. The format of this structure is one where the constants and variables are declared (for the most part) at the beginning of the file. This is also somewhat necessary since a constant or variable cannot be used by any part of the program before it has been defined (declared).
Declaration of Constants and Variables
The statements below show the definitions of various constants and variables. Most are assigned a value in their declaration.
//////////////////////////////////////////////////////////////////////
//
// Define constants and variables to be used in the functions below
//
typedef struct genericRobotStruct {
int powerLevel;
int wheelDiameterMM;
int gearRatio;
int distanceBetweenWheelsMM;
} robotObj; robotObj robot;// 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
#define untilTouch() if(OkToMove) { say("Push touch sensor to continue"); while(OkToMove && !SensorValue[touchSensor]) sleep(5); }
#define clearLCD() for(int lcdLine = 0; lcdLine<8; lcdLine++) displayTextLine(lcdLine, " ")
#define lightSensor(sensorName) getColorReflected(sensorName)Port Monitoring
/******************************************************************************************************
This task monitors the ports to make sure everything is connected
*******************************************************************************************************/
task portMonitor() {
while(true) {
if (SensorType(S1)==0 || SensorType(S2)==0 || SensorType(S3)==0 || SensorType(S4)==0) {
while(SensorType(S1)==0 || SensorType(S2)==0 || SensorType(S3)==0 || SensorType(S4)==0) {
eraseDisplay();
setLEDColor(ledRedFlash);
displayCenteredBigTextLine(2, "Sensor Port");
displayCenteredBigTextLine(4, "#%d is Unused.", (SensorType(S1)==0) ? 1: (SensorType(S2)==0) ? 2: (SensorType(S3)==0) ? 3: 4 );
displayCenteredBigTextLine(6, "May be Loose.");
displayCenteredBigTextLine(8, "Check Wiring.");
playTone(440,4); wait1Msec(600);
playTone(660,4); wait1Msec(600);
}
eraseDisplay();
setLEDColor(ledOff);
}
if (motorType(motorA)==0 || motorType(motorB)==0 || motorType(motorC)==0 || motorType(motorD)==0) {
while(motorType(motorA)==0 || motorType(motorB)==0 || motorType(motorC)==0 || motorType(motorD)==0) {
eraseDisplay();
setLEDColor(ledRedFlash);
displayCenteredBigTextLine(2, "Motor Port");
displayCenteredBigTextLine(4, "#%d is Unused.", (motorType(motorA)==0) ? 1: (motorType(motorB)==0) ? 2: (motorType(motorC)==0) ? 3: 4 );
displayCenteredBigTextLine(6, "May be Loose.");
displayCenteredBigTextLine(8, "Check Wiring.");
playTone(440,4); wait1Msec(600);
playTone(660,4); wait1Msec(600);
}
eraseDisplay();
setLEDColor(ledOff);
}
}
}
void checkPort(const tSensors nPort, TSensorTypes tst) {
if (SensorType(nPort)==0) {
eraseDisplay();
setLEDColor(ledRedFlash);
displayCenteredBigTextLine(2, (tst==sensorEV3_Touch ? "Touch Sensor": (tst==sensorEV3_Color) ? "Color Sensor": (tst==sensorEV3_Ultrasonic) ? "Ultrasonic": ""));
displayCenteredBigTextLine(4, "In Port 1 Could");
displayCenteredBigTextLine(6, "be Loose. Check");
displayCenteredBigTextLine(8, "the Cables");
playSoundFile("Woops"); wait1Msec(1000);
OkToMove = true;
}
}Driving Forward and Backward
While it is possible to declare all variables at the top of the file, it sometimes is easier to read and understand the program if variables are declared next to the functions that use them.
//////////////////////////////////////////////////////////////////////
//
// Simple Driving Functions
//
void drive() { if(!OkToMove) return; motor[B]=-20; motor[C]=-20; }
void driveForward(float distance) { if(OkToMove) forward(distance*30, degrees, robot.powerLevel); }
void driveForward() { if(OkToMove) setMultipleMotors(robot.powerLevel, B, C); } // Overloaded function
void driveBackward(float distance) { if(OkToMove) backward(distance*30, degrees, robot.powerLevel); }
void driveBackward() { if(OkToMove) setMultipleMotors(robot.powerLevel * -1, B, C); } // Overloaded functionAccessory Motor Functions
Purpose
//////////////////////////////////////////////////////////////////////
//
// Functions used to Control the Accessory Motors
void accessoryUp(float mdegrees, int tempPowerLevel) { if(!OkToMove) return;
long stallDetector;
long startingDegree; startingDegree=getMotorEncoder(A);
setMotor(A,-1 * tempPowerLevel);
while(OkToMove) {
stallDetector=getMotorEncoder(A);
sleep(100);
if(abs(stallDetector - getMotorEncoder(A)) < 10) break; // Stalled
if(mdegrees>0 && abs(startingDegree - getMotorEncoder(A)) >= mdegrees) break; // reached target
}
motor[A]=0;
}
void accessoryDown(float mdegrees, int tempPowerLevel) { if(!OkToMove) return;
long stallDetector;
long startingDegree; startingDegree=getMotorEncoder(A);
setMotor(A,tempPowerLevel);
while(OkToMove) {
stallDetector=getMotorEncoder(A);
sleep(100);
if(abs(stallDetector - getMotorEncoder(A)) < 10) break; // Stalled
if(mdegrees>0 && abs(startingDegree - getMotorEncoder(A)) >= mdegrees) break; // reached target
}
motor[A]=0;
}
void accessoryUp(float mdegrees) { if(!OkToMove) return; accessoryUp(mdegrees, 20); }
void accessoryUp() { if(OkToMove) accessoryUp(-1); }
void accessoryDown(float mdegrees) { if(!OkToMove) return; accessoryDown(mdegrees, 20); }
void accessoryDown() { if(OkToMove) accessoryDown(-1); }
void accessory2Up(float mdegrees, int tempPowerLevel) { if(!OkToMove) return;
long stallDetector;
long startingDegree; startingDegree=getMotorEncoder(ClipOn);
setMotor(ClipOn,-1 * tempPowerLevel);
while(OkToMove) {
stallDetector=getMotorEncoder(ClipOn);
sleep(100);
if(abs(stallDetector - getMotorEncoder(ClipOn)) < 10) break; // Stalled
if(mdegrees>0 && abs(startingDegree - getMotorEncoder(ClipOn)) >= mdegrees) break; // reached target
}
motor[ClipOn]=0;
}
void accessory2Down(float mdegrees, int tempPowerLevel) { if(!OkToMove) return;
long stallDetector; resetMotorEncoder(ClipOn);
long startingDegree; startingDegree=getMotorEncoder(ClipOn);
setMotor(ClipOn,tempPowerLevel);
while(OkToMove) {
stallDetector=getMotorEncoder(ClipOn);
sleep(100);
if(abs(stallDetector - getMotorEncoder(ClipOn)) < 10) break; // Stalled
if(mdegrees>0 && abs(startingDegree - getMotorEncoder(ClipOn)) >= mdegrees) break; // reached target
}
motor[ClipOn]=0;
}
void accessory2Up(float mdegrees) { if(!OkToMove) return; accessory2Up(mdegrees, 20); }
void accessory2Up() { if(OkToMove) accessory2Up(-1); }
void accessory2Down(float mdegrees) { if(!OkToMove) return; accessory2Down(mdegrees, 20); }
void accessory2Down() { if(OkToMove) accessory2Down(-1); }Primitive Stopping Functions
void stopMoving() { stopMultipleMotors(B, C); }
#define stopOnTouch() if(OkToMove) while(OkToMove && !SensorValue[touchSensor]) sleep(5); stopMoving()Turning Functions Defined with the Gyroscope
void untilGyro(float degrees) { if(!OkToMove) return;
float currDegree; currDegree=getGyroDegrees(gyroSensor);
sleep(50); currDegree=getGyroDegrees(gyroSensor);
repeatUntil(abs(getGyroDegrees(gyroSensor)-currDegree) > degrees || !OkToMove) {
sleep(5);
displayBigTextLine(3, "start: %d", (int) currDegree);
displayBigTextLine(5, "Sens: %d", (int) getGyroDegrees(gyroSensor));
displayBigTextLine(7, "Diff: %d", (int) getGyroDegrees(gyroSensor)-currDegree );
displayBigTextLine(9, "Abs: %d", (int) abs(getGyroDegrees(gyroSensor)-currDegree) );
}
stopMoving();
}
#define pointTurnClockwise(degrees) if(OkToMove) { motor[B]=-20; motor[C]=20; untilGyro(degrees-10); }
#define pointTurnCounterClockwise(degrees) if(OkToMove) { motor[B]=20;motor[C]=-20; untilGyro(degrees-10); }
#define swingTurnClockwise(degrees) if(OkToMove) { motor[B]=0; motor[C]=20; untilGyro(degrees-10); }
#define swingTurnCounterClockwise(degrees) if(OkToMove) {motor[B]=20; motor[C]=0; untilGyro(degrees-10); }
#define PI 3.14159Distance Travelled
////////////////////////////////////////////////////////////////////////////////////
//
void untilDistance(int moveIN) {
resetMotorEncoder(B); resetMotorEncoder(C);
float targetDegree;
targetDegree=(moveIN * 25.4)/(robot.wheelDiameterMM*PI)*360;
repeatUntil(getMotorEncoder(B) > targetDegree) { sleep(5); }
}Gradual Turning
//////////////////////////////////////////////////////////////////////
//
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);
stopMoving();
}
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);
stopMoving();
}
void gradualTurnClockwise(float degrees) { gradualTurnClockwise(degrees, 20); }
void gradualTurnCounterClockwise(float degrees) { gradualTurnCounterClockwise(degrees, 20); }Detecting Lines and Colors
//////////////////////////////////////////////////////////////////////
//
//
long redChannel; long greenChannel; long blueChannel;
void untilColor(TLegoColors pColor) { if(!OkToMove) return;
getColorRGB(colorSensor1, redChannel, greenChannel, blueChannel);
sleep(25);
switch(pColor) {
case colorRed: while(OkToMove && true) { getColorRGB(colorSensor1, redChannel, greenChannel, blueChannel); sleep(5); if(redChannel>40) break; } break;
case colorGreen: while(OkToMove && true) { getColorRGB(colorSensor1, redChannel, greenChannel, blueChannel); sleep(5); if(greenChannel>40) break; } break;
case colorBlue: while(OkToMove && true) { getColorRGB(colorSensor1, redChannel, greenChannel, blueChannel); sleep(5); if(blueChannel>40) break; } break;
case colorBlack: while(OkToMove && true) { getColorRGB(colorSensor1, redChannel, greenChannel, blueChannel); sleep(5); if(blueChannel<20 && greenChannel<20 && redChannel<20) break; } break;
case colorWhite: while(OkToMove && true) { getColorRGB(colorSensor1, redChannel, greenChannel, blueChannel); sleep(5); if(blueChannel>40 && greenChannel>40 && redChannel>40) break; } break;
}
}
#define stopOnColor(pColor) untilColor(pColor);stopAllMotors();Initializing the Speech Engine
/************************************************************************************/
#include "SpeechEngine.c";
/************************************************************************************/
The Menu
/************************************************************************************
// Menu items are comma separated
Usage: displayMenu("Item1, Item2, Item3, Item4");
Return value is the index of the item selected.
************************************************************************************/
typedef struct {
string Item;
} String;
int menuItemSelected = 0;
int displayMenu(const char *phrase, int startingMenuItem = 1) {
int currMenuItem=0;
int maxMenuItem=0;
startingMenuItem--; // The User function passes a 1-offset index number. Internal function needs a 0-offset index number
String Menu[10];
Menu[0].Item = "";
Menu[1].Item = "";
Menu[2].Item = "";
Menu[3].Item = "";
Menu[4].Item = "";
Menu[5].Item = "";
Menu[6].Item = "";
Menu[7].Item = "";
Menu[8].Item = "";
Menu[9].Item = "";
setLEDColor(ledOrange);
clearLCD();
//// Display first menu item in center of screen
// Display first menu item in center of screen
char tempPhrase[120]; // Temporary working space for the entire string
char partArray[25]; // Temporary working space for just the short words
strcpy(tempPhrase, (const char *) phrase); // Copy the passed string into the temporary working phrase
string part1; // Setup a string to store the resulting words
int menucharNdx; // Main index to point to characters in the Temporary working string
int partArrayNdx; // Secondary index to point to the characters in the resulting words
strcpy(part1, ""); // Clear it
partArrayNdx=0; // Initialize it
bool trimBlanks = true;
for(menucharNdx=0;menucharNdx<120;menucharNdx++) { // Look through the contents of the Temporary workng space
if(tempPhrase[menucharNdx]==',') { // If found a comma, then this word is a menu item
partArray[partArrayNdx]=0; // Strings must be terminated with a null character '0'
stringFromChars( part1, partArray);// Prepare a string from the working array of characters called partArray[]
Menu[maxMenuItem++].Item = part1; // Now store it
partArrayNdx=0; // reset the partArray index
trimBlanks = true; // Done with this item, better start trimming now
} else {
if(tempPhrase[menucharNdx]==0) { // If found the null character, then the string has ended
partArray[partArrayNdx]=0; // Terminate the string with a null character
stringFromChars( part1, partArray);// Move the characters onto a temporary string
Menu[maxMenuItem++].Item = part1; // Now store it
break;
} else {
if(tempPhrase[menucharNdx]==' ' && trimBlanks) { // If found the null character, then the string has ended
} else {
trimBlanks = false; // I have a character. You can stop trimming now
partArray[partArrayNdx++]=toLower(tempPhrase[menucharNdx]); // Convert to lower case and add it to the temp word
}
}
}
// setLEDColor(ledGreen);
}//
bool needToShowMenu = true;
currMenuItem=startingMenuItem;
while(getButtonPress(buttonEnter)) { sleep(50); } // Wait. If someone is already pressing the button, you need to wait more.
while(!getButtonPress(buttonEnter)) { // Wait Here
// Now you have the menu items
OkToMove = true;
if(needToShowMenu) {
clearLCD();
canTalkNow = true;
displayBigTextLine(1, " %s", "=== Menu ===");
if(currMenuItem>0) displayBigTextLine(5, " %s", Menu[currMenuItem-1].Item);
if(currMenuItem==0) displayBigTextLine(5," %s", " ");
displayBigTextLine(9, " %s", Menu[currMenuItem].Item);
if(currMenuItem<maxMenuItem) displayBigTextLine(13," %s", Menu[currMenuItem+1].Item);
if(currMenuItem==maxMenuItem) displayBigTextLine(13," %s", " ");
drawRect(1, 36, 170, 60);
needToShowMenu=false;
#ifdef SPEECH_ENGINE
say(Menu[currMenuItem].Item);
#else
playSoundFile("Click");
#endif
}
if (getButtonPress(buttonUp)) { currMenuItem=(currMenuItem>0)?currMenuItem-1:currMenuItem; needToShowMenu=true; }
if (getButtonPress(buttonDown)){ currMenuItem=(currMenuItem<maxMenuItem-1)?currMenuItem+1:currMenuItem; needToShowMenu=true; }
sleep(200);
}
setLEDColor(ledGreen);
playSoundFile("Confirm");
clearLCD(); // Get rid of the menu
sleep(1000);
setLEDColor(ledGreenFlash);
OkToMove = true;
menuItemSelected = currMenuItem;
return currMenuItem;
}task main()
{
robot.powerLevel=20; robot.wheelDiameterMM=92; robot.gearRatio=1; robot.distanceBetweenWheelsMM=136;
startTask(leftRightButtonHandler);
startTask(portMonitor); // Make sure there is something in every port
setBlockBackButton(true); // Prevent the Back button from enabling the program to be killed
while(true) {
OkToMove = true;
switch(displayMenu("one, turn on power, the toy, get building, seth sorter, turtle ,squid , compost ,windshield", (menuItemSelected+1))) {
case 0:
displayBigTextLine(2, "%s", "This is test 1");
untilTouch();
driveForward(20);
stopMoving();
if(OkToMove) {
resetMotorEncoder(B);
long startingEncoderValue = getMotorEncoder(B);
motor[motorB]=10; // Rotate the wheel forwards
while(abs(getMotorEncoder(B)-startingEncoderValue) < 90*3) {
}
stopMoving();
}
driveForward(5);
wait(1);
if(OkToMove) {
resetMotorEncoder(B);
long startingEncoderValue = getMotorEncoder(B);
motor[motorB]=10; // Rotate the wheel forwards
while(abs(getMotorEncoder(B)-startingEncoderValue) < 120*3.5) {
}
stopMoving();
}
driveForward(24);
untilTouch();
sayPhrase("Do you want to continue");
switch(displayMenu("no, yes")) {
case 0:
break;
case 1:
setPowerLevel(100);
driveForward(12);
}
//drive();
OkToMove=true;
while(OkToMove) {
int n;
int m;
n=lightSensor(colorSensor1);
m=lightSensor(colorSensor2);
displayBigTextLine(11," %d", n);
displayBigTextLine(13," %d", m);
motor[B]=n/3;
motor[C]=m/3;
}
untilTouch();
stopAllMotors();
break;//
case 1:
//accessoryUp();
//turn on power
driveForward(2);
if(OkToMove) {
resetMotorEncoder(C);
long startingEncoderValue = getMotorEncoder(C);
motor[motorC]=10; // Rotate the wheel forwards
while(abs(getMotorEncoder(C)-startingEncoderValue) < 45*2) {
}
stopMoving();
}
driveForward();
wait(1);
stopMoving();
if(OkToMove) {
resetMotorEncoder(B);
long startingEncoderValue = getMotorEncoder(B);
motor[motorB]=10; // Rotate the wheel forwards
while(abs(getMotorEncoder(B)-startingEncoderValue) < 45*2) {
}
stopMoving();
}
driveForward(10);
driveForward();
stopOnColor(colorBlack);
driveForward(2);
if(OkToMove) {
resetMotorEncoder(C);
long startingEncoderValue = getMotorEncoder(C);
motor[motorC]=10; // Rotate the wheel forwards
while(abs(getMotorEncoder(C)-startingEncoderValue) < 85*3) {
}
stopMoving();
}
driveForward(5);
driveBackward(10);
pointTurnCounterClockwise(90);
driveBackward(); // Toward Base
stopOnTouch();
say("Aim me toward the power");
wait(2);
break;
case 2:
// Just get the toy from the power plant
driveForward(12);
swingTurnCounterClockwise(45);
driveForward(10);
swingTurnCounterClockwise(45);
driveForward(12);
swingTurnCounterClockwise(45);
//driveForward(12); // Go get the toy
pointTurnClockwise(45); // return to base
driveBackward();
stopOnTouch();
break;//
case 3:
say("pushing the jig");
untilTouch();
driveForward(15);
// Slow down
setPowerLevel(10);
driveForward(6);
// Back to base
setPowerLevel(20);
driveBackward(15);
wait(3);
// Re-position the robot
untilTouch();
setPowerLevel(20);
driveForward(3); // Go out of base and go around the building
gradualTurnCounterClockwise(47, 6);
driveForward(5);
gradualTurnClockwise(47, 6);
driveForward(24);
gradualTurnClockwise(185,4);
// crash into building
setPowerLevel(40);
driveForward(4);
// back away
setPowerLevel(20);
driveBackward(8);
gradualTurnClockwise(70,6);
gradualTurnCounterClockwise(70,3);
driveForward(4);
gradualTurnCounterClockwise(20,3);
driveForward(18);
driveForward();
stopOnTouch();
break;//
case 4:
setMotorBrakeMode(clipOn, 0);
say("hello seth");
wait(1);
for(int sLoop = 0; sLoop<3; sLoop++) {
eraseDisplay();
displayBigTextLine(2,"HELLO SETH");
say("Hello Seth");
wait(1);
displayBigTextLine(5,"ATTACH THE");
wait(.5);
displayBigTextLine(8,"PARTS RAMP");
wait(.5);
eraseDisplay();
wait(1);
}
eraseDisplay();
untilTouch();
driveBackward();
sleep(300);
stopMoving();
wait(.5);
gradualTurnCounterClockwise(55,10);
driveForward(6);
gradualTurnClockwise(50,12);
driveForward(5);
driveForward();
// Look for first North-South line
repeatUntil(!OkToMove || lightSensor(colorSensor2)<25) { sleep(5); }
stopMoving();
driveForward(4);
// Rotate the robot CounterClockwise to face North
if(OkToMove) {
resetMotorEncoder(B);
long startingEncoderValue = getMotorEncoder(B);
motor[motorB]=10; // Rotate the wheel forwards
while(abs(getMotorEncoder(B)-startingEncoderValue) < 90 * 3) { } // Need to rotate about 30 degrees
stopMoving();
}
if(OkToMove) {
resetMotorEncoder(C);
long startingEncoderValue = getMotorEncoder(C);
motor[motorC]=-10; // Rotate the wheel backwards
while(abs(getMotorEncoder(C)-startingEncoderValue) < 10 * 3) { } // Need to rotate about 10 degrees
stopMoving();
}
driveBackward(4);
driveForward();//
// Look for the main East-West line
repeatUntil(!OkToMove || lightSensor(colorSensor1)<20) { sleep(5); }
stopMoving();
if(OkToMove) {
long startingEncoderValue;
resetMotorEncoder(B);
startingEncoderValue = getMotorEncoder(B);
motor[motorB]=-10; // Rotate the Right wheel backwards
while(abs(getMotorEncoder(B)-startingEncoderValue) < 5 * 3) { } // Need to rotate about 90 degrees face east
stopMoving();
resetMotorEncoder(C);
startingEncoderValue = getMotorEncoder(C);
motor[motorC]=10; // Rotate the left ?swheel forwards
while(abs(getMotorEncoder(C)-startingEncoderValue) < 85 * 3) { } // Need to rotate about 90 degrees face east
stopMoving();
}
driveForward(1);
// Dump the bars into the bin
accessory2Up(600);
wait(1);
accessory2Down(300, 80);
accessory2Up(300, 80);
wait(1);
accessory2Down(300, 80);
driveBackward(26);
if(OkToMove) {
resetMotorEncoder(C);
long startingEncoderValue = getMotorEncoder(C);
motor[motorC]=-10; // Rotate the left wheel backwards to back into base
startingEncoderValue = getMotorEncoder(C);
while(abs(getMotorEncoder(C)-startingEncoderValue) < 15 * 3) { } // Need to rotate about 90 degrees face east
stopMoving();
}
driveBackward(16);
break;//
case 5:
say("hello mister turtle");
accessoryUp();
driveForward(60); //turtle
accessoryDown();
driveBackward(15);
break;
case 6:
driveBackward();
accessoryUp();
driveForward(40);
accessoryDown();
accessoryUp();
driveForward(19);
wait(22);
driveBackward(64);
//driveForward(50);
//driveBackward(50);//squid
break;
case 7:
setPowerLevel(30);
sayPhrase("gyroscope");
driveForward(17);
pointTurnCounterClockwise(45);
driveForward(16);
pointTurnClockwise(45);
driveForward(2);
pointTurnCounterClockwise(90);
driveForward(1);
break;//
case 8:
say("REDUCE,REUSE,RECYCLE!");
long startingEncoderValue;
driveBackward();
sleep(300);
stopMoving();
wait(.5);
gradualTurnCounterClockwise(55,10);
gradualTurnClockwise(50,12);
driveForward(10);
driveForward();
// Look for first North-South line
repeatUntil(!OkToMove || lightSensor(colorSensor2)<20) { sleep(5); }
untilDistance(10);
stopMoving();
// Rotate the robot CounterClockwise to face the Sorter
if(OkToMove) {
resetMotorEncoder(B);
motor[motorB]=10; // Rotate the wheel forwards
long startingEncoderValue = getMotorEncoder(B);
while(abs(getMotorEncoder(B)-startingEncoderValue) < 20 * 3) { } // Need to rotate about 30 degrees
stopMoving();
}
driveForward();
// Look for main East-West line. Also, make sure you travelled at least half a turn
resetMotorEncoder(B);
startingEncoderValue = getMotorEncoder(B);
repeatUntil(!OkToMove || (lightSensor(colorSensor2)<14 && getMotorEncoder(B)-startingEncoderValue > 180)) {
sleep(5);
}
stopMoving();
swingTurnClockwise(20);
if(OkToMove) {
resetMotorEncoder(B);
startingEncoderValue = getMotorEncoder(B);
repeatUntil(!OkToMove || (lightSensor(colorSensor1)<20 && getMotorEncoder(B)-startingEncoderValue > 360)) {
lineTrackRight(colorSensor2, 8, 15, 3);
}
}
stopMoving();
if(OkToMove) {
motor[motorD] = -10;
sleep(3000);
stopMultipleMotors(A, ClipOn);
}
break;
}
}
}