#pragma systemFile // this eliminates warning for "unreferenced" functions
#define print writeDebugStreamLine
#define clear clearDebugStream
#define setLED setLEDColor
#define drawRectangle(Left, Bottom, Width, Height) drawRect(Left, Bottom+Height, Left+Width, Bottom)
#define eraseRectangle(Left, Bottom, Width, Height) eraseRect(Left, Bottom+Height, Left+Width, Bottom)
#define fillRectangle(Left, Bottom, Width, Height) fillRect(Left, Bottom+Height, Left+Width, Bottom)
#define fillCircle(CenterX, CenterY, Radius) fillEllipse(CenterX-Radius, CenterY+Radius, CenterX+Radius, CenterY-Radius)
#define eraseCircle(CenterX, CenterY, Radius) eraseEllipse(CenterX-Radius, CenterY+Radius, CenterX+Radius, CenterY-Radius)
#define clearDisplay eraseDisplay
#define sine(Degrees) sinDegrees(Degrees)
#define cosine(Degrees) cosDegrees(Degrees)
#define random(Limit) abs(rand()%Limit)
#define getColorIndex(Sensor) getColorName(Sensor)
//#define DEBUGSTREAM_MESSAGES_FOR_TURNING
// To silence the robot
// #define SILENCE
// To use the library's Motor Synchronization programs
// #define USE_MANUAL_SYNCHRONIZATION
//####################################################
// Graphics Library Structures
typedef struct coord_struct { int x; int y; } coordinate;
typedef enum ObjectClasses { CIRCLE_OBJECT, SQUARE_OBJECT, RECTANGLE_OBJECT } ObjectClass;
typedef enum offSides_struct { TOP_SIDE, RIGHT_SIDE, BOTTOM_SIDE, LEFT_SIDE } offSides;
typedef struct genericObject_struct {
bool created; int index; ObjectClass objClass;
coordinate center; coordinate lowerLeft;
int radius; int size; int width; int height;
int vector; int velocity;
bool filled; int mass;
bool offDisplay;
offSides offAtSide;
bool permitErase;
} genericObject;
//####################################################
typedef int aCircle;
typedef int obj;
genericObject object[100];
void eraseObject(obj myObject) {
if(!object[myObject].permitErase) return;
switch((ObjectClass) object[myObject].objClass) {
case CIRCLE_OBJECT:
eraseEllipse(object[myObject].center.x-object[myObject].radius,
object[myObject].center.y+object[myObject].radius,
object[myObject].center.x+object[myObject].radius,
object[myObject].center.y-object[myObject].radius);
break;
case SQUARE_OBJECT:
eraseRectangle(object[myObject].lowerLeft.x, object[myObject].lowerLeft.y, object[myObject].size, object[myObject].size);
break;
case RECTANGLE_OBJECT:
eraseRectangle(object[myObject].lowerLeft.x, object[myObject].lowerLeft.y, object[myObject].width, object[myObject].height);
break;
}
}
void drawObject(obj myObject) {
switch((ObjectClass) object[myObject].objClass) {
case CIRCLE_OBJECT:
if(object[myObject].filled) {
fillEllipse(object[myObject].center.x-object[myObject].radius,
object[myObject].center.y+object[myObject].radius,
object[myObject].center.x+object[myObject].radius,
object[myObject].center.y-object[myObject].radius);
} else {
drawEllipse(object[myObject].center.x-object[myObject].radius,
object[myObject].center.y+object[myObject].radius,
object[myObject].center.x+object[myObject].radius,
object[myObject].center.y-object[myObject].radius);
}
break;
case SQUARE_OBJECT:
if(object[myObject].filled) {
fillRectangle(object[myObject].lowerLeft.x, object[myObject].lowerLeft.y, object[myObject].size, object[myObject].size);
} else {
drawRectangle(object[myObject].lowerLeft.x, object[myObject].lowerLeft.y, object[myObject].size, object[myObject].size);
}
break;
case RECTANGLE_OBJECT:
if(object[myObject].filled) {
fillRectangle(object[myObject].lowerLeft.x, object[myObject].lowerLeft.y, object[myObject].width, object[myObject].height);
} else {
drawRectangle(object[myObject].lowerLeft.x, object[myObject].lowerLeft.y, object[myObject].width, object[myObject].height);
}
break;
}
}
void translate(obj targetObject, int x, int y) {
eraseObject(targetObject);
switch((ObjectClass) object[targetObject].objClass) {
case CIRCLE_OBJECT:
object[targetObject].center.x = x;
object[targetObject].center.y = y;
break;
case SQUARE_OBJECT:
object[targetObject].lowerLeft.x = x;
object[targetObject].lowerLeft.y = y;
break;
case RECTANGLE_OBJECT:
object[targetObject].lowerLeft.x = x;
object[targetObject].lowerLeft.y = y;
break;
}
drawObject(targetObject);
}
int mirrorVectorVertical(int vector) {
int newVector=0;
int tVector = (vector + 360) % 360;
writeDebugStreamLine("tVector %d", tVector);
if(tVector>270) newVector= (180-tVector) % 360;
if(tVector>180) newVector= ((360-(tVector-180))+360) % 360;
if(tVector> 90) newVector= ((360-tVector)+360) % 360;
newVector= ((180-tVector)+360) % 360;
if(newVector < 0) newVector+=360;
return newVector % 360;
}
int mirrorVectorHorizontal(int vector) {
int newVector=0;
int tVector = (vector + 360) % 360;
//if(tVector<180) return 180-tVector;
writeDebugStreamLine("tVector %d", tVector);
newVector=((360-tVector) + 360) % 360;
if(newVector < 0) newVector+=360;
return newVector % 360;
}
float distanceBetween(int x1, int y1, int x2, int y2) {
return sqrt((x1-x2)+(y1-y2));
}
void haveCollided(obj Obj1, obj Obj2) {
if((ObjectClass) object[Obj1].objClass == CIRCLE_OBJECT) {
if((ObjectClass) object[Obj2].objClass == CIRCLE_OBJECT) {
// Collision between two circles
float distance = distanceBetween(object[Obj1].center.x, object[Obj1].center.y, object[Obj2].center.x, object[Obj2].center.y);
if(distance < object[Obj1].radius + object[Obj2].radius) {
// Since the distance is less than the sum of the radii, try to re-position the objects to the point of collision
}
}
}
}
void copyObject(obj myObject, genericObject *objPtr) {
objPtr->objClass = object[myObject].objClass;
objPtr->center.x = object[myObject].center.x;
objPtr->center.y = object[myObject].center.y;
objPtr->lowerLeft.x = object[myObject].lowerLeft.x;
objPtr->lowerLeft.y = object[myObject].lowerLeft.y;
objPtr->radius = object[myObject].radius;
objPtr->size = object[myObject].size;
objPtr->width = object[myObject].width;
objPtr->height = object[myObject].height;
objPtr->filled = object[myObject].filled;
objPtr->vector = object[myObject].vector;
objPtr->velocity = object[myObject].velocity;
objPtr->permitErase = object[myObject].permitErase;
}
bool isOffDisplay(obj myObject) {
bool offDisplay=false; offDisplay=false;
genericObject tObj;
copyObject(myObject, &tObj);
tObj = object[myObject];
tObj.vector %= 360;
//displayBigTextLine(13,"%4d", tObj.center.x);
switch((ObjectClass) tObj.objClass) {
case CIRCLE_OBJECT:
if(tObj.center.x-tObj.radius <= 0 && tObj.vector >= 90 && tObj.vector <= 270) { offDisplay=true; object[myObject].offAtSide = LEFT_SIDE; }
if(tObj.center.x+tObj.radius > 180 && (tObj.vector < 90 || tObj.vector > 270)) { offDisplay=true; object[myObject].offAtSide = RIGHT_SIDE; }
if(tObj.center.y-tObj.radius <= 0 && tObj.vector >= 180) { offDisplay=true; object[myObject].offAtSide = BOTTOM_SIDE; }
if(tObj.center.y+tObj.radius > 125 && tObj.vector < 180) { offDisplay=true; object[myObject].offAtSide = TOP_SIDE; }
break;
case SQUARE_OBJECT:
if(object[myObject].lowerLeft.x < 0 || object[myObject].lowerLeft.x+object[myObject].size > 175) { offDisplay=true; }
if(object[myObject].lowerLeft.y < 0 || object[myObject].lowerLeft.y+object[myObject].size > 125) { offDisplay=true; }
break;
case RECTANGLE_OBJECT:
if(object[myObject].lowerLeft.x < 0 || object[myObject].lowerLeft.x+object[myObject].width > 175) { offDisplay=true; }
if(object[myObject].lowerLeft.y < 0 || object[myObject].lowerLeft.y+object[myObject].height > 125) { offDisplay=true; }
break;
}
return offDisplay;
}
void moveObject(obj myObject) {
if(object[myObject].velocity>0) {
// erase current object
// calculate the new position based on vector and velocity
switch((ObjectClass) object[myObject].objClass) {
case CIRCLE_OBJECT:
eraseEllipse(object[myObject].center.x-object[myObject].radius,
object[myObject].center.y+object[myObject].radius,
object[myObject].center.x+object[myObject].radius,
object[myObject].center.y-object[myObject].radius);
object[myObject].center.y+=(int)(float)object[myObject].velocity * sine(object[myObject].vector);
object[myObject].center.x+=(int)(float)object[myObject].velocity * cosine(object[myObject].vector);
break;
case SQUARE_OBJECT:
eraseRectangle(object[myObject].lowerLeft.x, object[myObject].lowerLeft.y, object[myObject].size, object[myObject].size);
object[myObject].lowerLeft.y+=(int)object[myObject].velocity * sine(object[myObject].vector);
object[myObject].lowerLeft.x+=(int)object[myObject].velocity * cosine(object[myObject].vector);
break;
case RECTANGLE_OBJECT:
eraseRectangle(object[myObject].lowerLeft.x, object[myObject].lowerLeft.y, object[myObject].width, object[myObject].height);
object[myObject].lowerLeft.y+=(int)object[myObject].velocity * sine(object[myObject].vector);
object[myObject].lowerLeft.x+=(int)object[myObject].velocity * cosine(object[myObject].vector);
break;
}
}
// redraw object
drawObject(myObject);
object[myObject].offDisplay = isOffDisplay(myObject);
}
void clearObjects() {
for(int i=0;i<100;i++) {
object[i].created = false;
object[i].index = i;
object[i].center.x = 0;
object[i].center.y = 0;
object[i].radius = 0;
object[i].vector = 0;
object[i].velocity = 0;
object[i].mass = 0;
object[i].filled = false;
object[i].permitErase = true;
}
}
obj newCircle(int CenterX, int CenterY, int Radius) {
int i;
for(i=0; i<100; i++) { if(!object[i].created) break; }
object[i].created=true;
object[i].objClass = CIRCLE_OBJECT;
object[i].center.x = CenterX;
object[i].center.y = CenterY;
object[i].radius = Radius;
object[i].permitErase = true;
return i;
}
obj newSquare(int lowerLeftX, int lowerLeftY, int Size) {
int i;
for(i=0; i<100; i++) { if(!object[i].created) break; }
object[i].created=true;
object[i].objClass = SQUARE_OBJECT;
object[i].lowerLeft.x = lowerLeftX;
object[i].lowerLeft.y = lowerLeftY;
object[i].size = Size;
object[i].permitErase = true;
return i;
}
obj newRectangle(int lowerLeftX, int lowerLeftY, int Width, int Height) {
int i;
for(i=0; i<100; i++) { if(!object[i].created) break; }
object[i].created=true;
object[i].objClass = RECTANGLE_OBJECT;
object[i].lowerLeft.x = lowerLeftX;
object[i].lowerLeft.y = lowerLeftY;
object[i].width = Width;
object[i].height = Height;
object[i].permitErase = true;
return i;
}
// #define drawCircle(CenterX, CenterY, Radius) drawEllipse(CenterX-Radius, CenterY+Radius, CenterX+Radius, CenterY-Radius)
#ifndef BasicPragmasDefined
int touchSensor1;
#endif
#ifndef ColorSensorDefined
int colorSensor;
#endif
#ifndef UltrasonicSensorDefined
int ultrasonicSensor;
#endif
//int colorSensor;
//int gyroSensor;
int touchSensorB;
int touchSensor2;
int lightSensorA;
int lightSensorB;
/////////////////////////////////////////////
// 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.
--*/
void errorMsg(string *myString) {
setLEDColor(ledRed);
clearDebugStream();
writeDebugStreamLine("%s", myString);
sleep(100000);
}
string Colors[8] = { "Nothing", "Black", "Blue", "Green", "Yellow", "Red", "White", "Brown"};
/*----
/////////////////////////////////////////////
Variables used in Line Following
Line following constants used to tell which side of line to follow
You tell the robot to follow the line on the Leftside, Rightside, or the Middle
using functions such as: followLine(LEFTSIDE);
----*/
typedef enum Side { LEFTSIDE=-1, MIDDLE, RIGHTSIDE };
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, you may have driven too fast over the line
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;
TLegoColors color;
int criteria;
Side 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.
task checkForMotorStall() {
while(true) {
sleep(100);
if(robotPaused) continue;
if(IsFollowingLine) continue;
hogCPU();
for(int thisMotor=0;thisMotor<=1;thisMotor++) {
if(abs(driveMotors.motorNum[thisMotor].lastEncoderValue - nMotorEncoder[driveMotors.motorNum[thisMotor].port]) < 5) {
if(abs(motor[driveMotors.motorNum[thisMotor].port]) > 0 )driveMotors.motorNum[thisMotor].stallCounter++; // If the absolute value of the motor's power is > 0 then we must be stalled? Increment the stall counter.
driveMotors.motorNum[thisMotor].stallCounter++; // Don't bother with the Power Level. Can't seem to read it anyhow
if(driveMotors.motorNum[thisMotor].stallCounter>5) { driveMotors.motorNum[thisMotor].stalled = true; driveMotors.motorNum[thisMotor].moving = false; }
} else {
driveMotors.motorNum[thisMotor].lastEncoderValue = nMotorEncoder[driveMotors.motorNum[thisMotor].port];
if(driveMotors.motorNum[thisMotor].stallCounter>0) { driveMotors.motorNum[thisMotor].stallCounter=0; }
driveMotors.motorNum[thisMotor].moving = true;
}
}
for(int thisMotor=0;thisMotor 0 ) {
//writeDebugStreamLine("Incrementing Stall Counter %1d", thisMotor);
accessoryMotors.motorNum[thisMotor].stallCounter++; // If the absolute value of the motor's power is > 0 then we must be stalled? Increment the stall counter.
}
accessoryMotors.motorNum[thisMotor].stallCounter++; // Don't bother with the power level. Can't seem to read it anyhow
if(accessoryMotors.motorNum[thisMotor].stallCounter>5) { accessoryMotors.motorNum[thisMotor].stalled = true; accessoryMotors.motorNum[thisMotor].moving = false; }
//writeDebugStreamLine("Motor Speed is: %3d", (int) abs(accessoryMotors.motorNum[thisMotor].speed));
//writeDebugStreamLine("Motor Is Stopped");
} else {
accessoryMotors.motorNum[thisMotor].lastEncoderValue = nMotorEncoder[currMotorPort];
if(accessoryMotors.motorNum[thisMotor].stallCounter>0) {
accessoryMotors.motorNum[thisMotor].stallCounter=0;
//writeDebugStreamLine("Setting Stall Counter to zero");
}
accessoryMotors.motorNum[thisMotor].moving = true;
//writeDebugStreamLine("Motor Speed is: %3d", (int) abs(accessoryMotors.motorNum[thisMotor].speed));
//writeDebugStreamLine("Motor Is Moving");
}
}
releaseCPU();
}
}
void resetMotorStallChecker() {
driveMotors.motorNum[0].stallCounter = 0;
driveMotors.motorNum[1].stallCounter = 0;
driveMotors.motorNum[0].stalled = false;
driveMotors.motorNum[1].stalled = false;
driveMotors.motorNum[0].moving = false;
driveMotors.motorNum[1].moving = false;
driveMotors.motorNum[0].speed = motor[driveMotors.motorNum[0].port];
driveMotors.motorNum[1].speed = motor[driveMotors.motorNum[1].port];
accessoryMotors.motorNum[0].stallCounter = 0;
accessoryMotors.motorNum[1].stallCounter = 0;
accessoryMotors.motorNum[0].stalled = false;
accessoryMotors.motorNum[1].stalled = false;
accessoryMotors.motorNum[0].moving = false;
accessoryMotors.motorNum[1].moving = false;
accessoryMotors.motorNum[0].speed = motor[accessoryMotors.motorNum[0].port];
accessoryMotors.motorNum[1].speed = motor[accessoryMotors.motorNum[1].port];
}
void configureAccessoryMotor(int port, OrientationType orientation, int tempPower) {
accessoryMotors.motorNum[accessoryMotors.count].port=port;
accessoryMotors.motorNum[accessoryMotors.count].orientation=orientation;
accessoryMotors.motorNum[accessoryMotors.count].power=tempPower;
accessoryMotors.motorNum[accessoryMotors.count].error=0;
if(accessoryMotors.powerLevel==0) accessoryMotors.powerLevel=tempPower;
accessoryMotors.count++;
resetMotorEncoder(port);
}
/////////////////////////////////////////////
// Moving Constants
const int STOPPED_MOVING = 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 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
//
// If the robot is following a line, there is extra code in the While() loop in the
// stopOnTouch(), stopAfterDistance(), and stopIfCloseTo()
/////////////////////////////////////////////
// Moving Primitives
void powerZero() {
motor[motorA] = 0; // turn the motor off.
motor[motorB] = 0; // turn the motor off.
motor[motorC] = 0; // turn the motor off.
motor[motorD] = 0; // turn the motor off.
driveMotors.motorNum[0].speed = motor[driveMotors.motorNum[0].port];
driveMotors.motorNum[1].speed = motor[driveMotors.motorNum[1].port];
accessoryMotors.motorNum[0].speed = motor[accessoryMotors.motorNum[0].port];
accessoryMotors.motorNum[1].speed = motor[accessoryMotors.motorNum[1].port];
}
void stopMoving() {
bFloatDuringInactiveMotorPWM = false; // brake
powerZero();
driveMotors.direction = STOPPED_MOVING;
IsFollowingLine = false; // Stopping means you are not following any line
}
/////////////////////////////////////////////
// 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.
const int CLOCKWISE = 1;
const int COUNTERCLOCKWISE = -1;
//
//
/////////////////////////////////////////////
// Variables to handle possible slack in the gear.
float motorError[] = {0, 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) {
nMotorEncoder[thisMotor] = 0;
resetMotorEncoder(thisMotor);
driveMotors.synched = false;
// if(thisMotor==motorA) { motorError[1]=0; }
// if(thisMotor==motorB) { motorError[2]=0; }
// if(thisMotor==motorC) { motorError[3]=0; }
// if(thisMotor==motorD) { motorError[4]=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 configureDriveMotors(int motor1, int motor2, OrientationType upsideDown) {
driveMotors.motorNum[0].port=motor1; driveMotors.motorNum[0].orientation=upsideDown;
driveMotors.motorNum[1].port=motor2; driveMotors.motorNum[1].orientation=upsideDown;
driveMotors.direction = STOPPED_MOVING; // Initial direction of the robot is set to Stopped
driveMotors.lastDirection = STOPPED_MOVING; // Initial direction of the robot is set to Stopped
driveMotors.orientation = upsideDown;
driveMotors.master = motor1;
driveMotors.slave = motor2;
driveMotors.synched = false;
resetMotorEncoder(motorB);
resetMotorEncoder(motorC);
}
void synchMotors(int master, float turnRatio) {
if(master == (int) driveMotors.motorNum[0].port) {
driveMotors.master=driveMotors.motorNum[0].port;
driveMotors.slave=driveMotors.motorNum[1].port;
driveMotors.synched = true;
//PlaySoundFile("OK.rso");
//nSyncedMotors = synchBC;
}
if(master == (int) driveMotors.motorNum[1].port) {
driveMotors.master=driveMotors.motorNum[1].port;
driveMotors.slave=driveMotors.motorNum[0].port;
driveMotors.synched = true;
//PlaySoundFile("OK.rso");
//nSyncedMotors = synchCB;
}
}
void synchMotors(int master) {
synchMotors(master, 0);
}
void nxtDisplayTextLine(int lineNum, string *stringFormat, string *sgb) {
writeDebugStreamLine(*stringFormat, *sgb);
}
void nxtDisplayCenteredTextLine(int lineNum, string *sgb) {
writeDebugStreamLine(*sgb);
}
void display(int lineNum, string *sgb) { // Display a message on a certain line
displayBigTextLine(lineNum*2, "%s", sgb);
}
void displayWithStream(int lineNum, string *sgb) { // Display a message on a certain line
displayBigTextLine(lineNum*2, "%s", sgb);
writeDebugStreamLine("%s", sgb);
say((char *) sgb);
}
void sayWithStream(int lineNum, string *sgb) { // Display a message on a certain line
displayBigTextLine(lineNum*2, "%s", sgb);
writeDebugStreamLine("%s", sgb);
say((char *) sgb);
}
void displayColorChannels(long RedChannel, long GreenChannel, long BlueChannel) {
writeDebugStreamLine("*RGB: %3d %3d %3d *", RedChannel, GreenChannel, BlueChannel);
displayBigTextLine(8, "Red: %3d", RedChannel);
displayBigTextLine(10, "Green: %3d", GreenChannel);
displayBigTextLine(12, "Blue: %3d", BlueChannel);
}
bool nNxtButtonPressed() {
return SensorValue[touchSensor1] ? true : false;
}
void waitForEnterButton() {
while(robotPaused) {}
if(menu.returnNow) return;
setLED(ledOrange);
#ifndef SILENCE
#ifdef SPEECH_ENGINE
say("Press Square Button Now");
#endif
#endif
while(!getButtonPress(buttonEnter)) {
if(menu.returnNow) return;
wait1Msec(10);
}
resetRotationSensor(motorB, motorC);
setLED(ledOff);
}
void waitForOrangeButton() {
while(robotPaused) {}
if(menu.returnNow) return;
setLED(ledOrange);
#ifndef SILENCE
#ifdef SPEECH_ENGINE
sayPhrase("Press Square Button Now");
#endif
#endif
while(!getButtonPress(buttonEnter)) {
if(menu.returnNow) return;
wait1Msec(10);
}
resetRotationSensor(motorB, motorC);
setLED(ledOff);
}
void wait(float time) {
while(robotPaused) {}
if(menu.returnNow) return;
wait1Msec(time * 1000);
}
void waitForTouchSensor() {
while(robotPaused) {}
if(menu.returnNow) return;
display(6,"*Press Touch Snsr"); // Notify User
display(7,"* to continue. *");
setLEDColor(ledOrange);
#ifndef SILENCE
#ifdef SPEECH_ENGINE
sayPhrase("Press Touch Sensor");
#endif
#endif
while(SensorValue(touchSensor1) == 0)
{ // Wait Here
if(menu.returnNow) return;
wait1Msec(10);
}
setLEDColor(ledOff);
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) {
writeDebugStreamLine("ERROR: Over Max\nfor nMotorEncoder\nPower down Robot.\nReset the encoder\nin your program.");
waitForOrangeButton();
}
}
//////////////////////////////////////////////////////////////////////
//
// Each robot may be constructed differently (wheel diameter, dimensions, ...)
// In order for the same functions to result in the same performance on every robot, it is necessary to
// obtain the physical dimensions of the robot. These dimensions are stored in a variables used in this file.
// The variables are defined and then initialized through one of the two functions below.
// A default set of values is stored in a text file located on the robot itself. The purpose of storing them
// in a text file is that it is a more permanent storage. This same file is read by all programs since each program
// must have the InitMove() function. The text file is created using the function: createRobotDimensionsConfig().
// If this file does not exist on the robot (as when you download the firmware), the robot will create one upon initialization.
//
void InitMove() {
/*-----
CloseAllHandles(nIoResult);
hFileHandle = 0; nFileSize = defaultFileSize;
OpenRead( hFileHandle, nIoResult, sFileNameRD, nFileSize);
if(nIoResult==ioRsltFileNotFound) {
createRobotDimensionsConfig(1); // Create the file and try again
OpenRead( hFileHandle, nIoResult, sFileNameRD, nFileSize);
}
ReadFloat(hFileHandle, nIoResult, centerOfWheelToCenterOfRobotMM);
ReadFloat(hFileHandle, nIoResult, wheelDiameterMM);
ReadFloat(hFileHandle, nIoResult, wheelCircumferenceMM);
ReadFloat(hFileHandle, nIoResult, gearRatio);
Close(hFileHandle, nIoResult);
------*/
// Also setup a few sensors
SensorType[touchSensor1] = sensorTouch; // Set 'touchSensor' to be of type sensorTouch
SensorType[ultrasonicSensor] = sensorSONAR; // Set 'sonarSensor' to be of type sensorSONAR
//SensorType[lightSensorA] = sensorLightInactive;
//SensorType[lightSensorB] = sensorLightInactive;
SensorType[colorSensor] = sensorLightInactive;
}
void configureWheelCircumferenceMM() {
config.wheelCircumferenceMM = config.wheelDiameterMM * PI; // Formula. NOT changeable.
}
//
// 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 getColorPercent(TLegoColors targetColor, tSensors nDeviceIndex) {
float result;
long RedChannel;
long GreenChannel;
long BlueChannel;
writeDebugStreamLine( "\n--------------\ngetColorPercent();");
getColorRGB(nDeviceIndex, RedChannel, GreenChannel, BlueChannel);
switch(lineFollower.color) {
case colorBlack:
result = (float) 100.0 - ((RedChannel + GreenChannel + BlueChannel) / 3.0);
break;
case colorRed:
result = (float) RedChannel;
break;
case colorGreen:
result = (float) GreenChannel;
break;
case colorBlue:
result = (float) BlueChannel;
break;
case colorWhite:
result = (float) (RedChannel + GreenChannel + BlueChannel) / 3.0;
break;
}
writeDebugStreamLine( "Result is: %5.2f", result);
if(result < 0) { result=0; }
if(result > 100) { result=100; }
return result;
}
float getColorPercent(TLegoColors targetColor) {
return getColorPercent(targetColor, colorSensor);
}
float lfColorPercent() {
float result;
long RedChannel;
long GreenChannel;
long BlueChannel;
getColorRGB(lineFollower.port, RedChannel, GreenChannel, BlueChannel);
switch(lineFollower.color) {
case colorBlack:
result = (float) (RedChannel + GreenChannel + BlueChannel) / 3.0;
break;
case colorRed:
result = (float) (GreenChannel + BlueChannel) / 2.0;
break;
case colorGreen:
result = (float) (RedChannel + BlueChannel) / 2.0;
break;
case colorBlue:
result = (float) (RedChannel + GreenChannel) / 2.0;
break;
}
result = result / 7.0 * 10.0;
if(result < 0) { result=0; }
if(result > 100) { result=100; }
writeDebugStreamLine( "lfColorPercent.color: %d Result is: %5.2f", lineFollower.color, result);
return result;
}
float getLightDiffPortion() {
float result;
long numerator1;
long numerator2;
long numerator;
numerator1 = (float) SensorRaw[lineFollower.port];
numerator2 = (float) SensorRaw[lineFollower.port];
numerator = (float) numerator1 - numerator2 + 100;
result = (float) numerator / 200;
return result;
}
/*-----
// The color sensor can be configured to recognize light in 5 different modes.
0 - modeEV3Color_Reflected
1 - modeEV3Color_Ambient
2 - modeEV3Color_Color
3 - modeEV3Color_Reflected_Raw
4 - modeEV3Color_RGB_Raw
These modes are set in the Pragma at the top of the program.
However, various "get" functions will re-set the mode of the color sensor. So, it is important to use the proper "get"
function for the mode you are trying to operate in. The "getColorIndex()" function will return the color in the form of
an enumerated value. The return value is an Integer. If you are trying to detect lines on the board, you may need to
slow the robot down so that enough time is spent over the line to detect it.
----*/
TLegoColors getColor() {
return (TLegoColors) getColorIndex(colorSensor);
}
//////////////////////////////////////////////////////////////////////
//
// 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 = 1.0; // Used as a multiplier to change the power to the motors from + to - if going in reverse.
if(driveMotors.direction == BACKWARD) { dFactor = -1.0;}// If not going forward, then you must be in reverse.
float variablePower = driveMotors.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 = ((lfColorPercent() - (float) 50) / (float) 50 ) * variablePower; // Calculates Kp as a percentage of the variablePower amount.
if(lineFollower.sideOfLine == RIGHTSIDE) { Kp *= -1; } // Switch the sign of Kp if you want to follow the other side of the line.
motor[driveMotors.master] = (int) (dFactor * (float) ((float) driveMotors.powerLevel + Kp)); // Add Kp to motor1 whether going forward or backward
motor[driveMotors.slave] = (int) (dFactor * (float) ((float) driveMotors.powerLevel - Kp)); // Subtract Kp from motor2 whether going forward or backward
//writeDebugStreamLine("ColorPct: %7.3f Master: %3d", lfColorPercent(), motor[driveMotors.master]); // 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) {
if(menu.returnNow) return;
writeDebugStreamLine( "\n--------------\nsetPowerLevel(%d);", power);
driveMotors.powerLevel = power; // Set the global variable to the value that was passed
driveMotors.powerLevelRev = -power; // Set the reverse power variable to the negative of the value that was passed
}
void setAccessoryPowerLevel(int power) {
if(menu.returnNow) return;
accessoryMotors.powerLevel = power; // Set the global variable for the Accessory Power Level
}
void setDefaultSpeed(int power) { setPowerLevel(power); }
void beginDrivingForward(int tempPower) {
while(robotPaused) {}
if(menu.returnNow) return;
clearDisplay();
display(2, "Begin Driving");
display(3, " Forward");
//displayBigTextLine(8, " at %3d power", tempPower);
writeDebugStreamLine( "\n--------------\nbeginDrivingForward();");
if(driveMotors.lastDirection == BACKWARD) { resetRotationSensor(motorB, motorC); }
driveMotors.direction = FORWARD; // Set the direction so other functions know which way we are going
driveMotors.lastDirection= driveMotors.direction;
driveMotors.tempPowerLevel = tempPower;
IsFollowingLine = false; // Just driving forward is not following any line
//writeDebugStreamLine( "Ready to set motor power");
//writeDebugStreamLine( "Motor[0] has port: %d" , (int) driveMotors.motorNum[0].port);
// We're going to use motor synchronization. Third parameter is a zero to drive straight. Fourth parameter is the target distance set to inifinity by this function. Other functions will control the stopping of the robot.
setMotorSyncEncoder( driveMotors.motorNum[0].port, driveMotors.motorNum[1].port, 0, 999999, driveMotors.direction * tempPower * driveMotors.orientation);
//motor[driveMotors.motorNum[0].port] = driveMotors.direction * tempPower * driveMotors.orientation;
//motor[driveMotors.motorNum[1].port] = driveMotors.direction * tempPower * driveMotors.orientation;
writeDebugStreamLine( "\ndriveMotorPower = %d;", driveMotors.direction * tempPower * driveMotors.orientation);
writeDebugStreamLine( "Power for motors is currently:\nMotor[0] = %d; Motor[1] = %d;", motor[driveMotors.motorNum[0].port], motor[driveMotors.motorNum[1].port]);
resetMotorStallChecker();
}
void beginDrivingBackward(int tempPower) {
while(robotPaused) {}
if(menu.returnNow) return;
clearDisplay();
displayWithStream(2, "Begin Driving");
sayWithStream(3, " Backward");
//displayBigTextLine(8, " at %3d power", tempPower);
// writeDebugStreamLine( "\n--------------\nbeginDrivingBackward();");
if(driveMotors.lastDirection == FORWARD) { resetRotationSensor(motorB, motorC); }
driveMotors.direction = BACKWARD; // Set the direction so other functions know which way we are going
driveMotors.lastDirection= driveMotors.direction;
driveMotors.tempPowerLevel = tempPower;
IsFollowingLine = false; // Just driving backward is not following any lines
//synchMotors(driveMotors.master); // Only the master motor needs power since the other moter is synched to the master
// writeDebugStreamLine( "Direction: %d",driveMotors.direction);
// writeDebugStreamLine( "TempPower: %d",tempPower);
// writeDebugStreamLine( "Oriented : %d",driveMotors.orientation);
// We're going to use motor synchronization. Third parameter is a zero to drive straight. Fourth parameter is the target distance set to inifinity by this function. Other functions will control the stopping of the robot.
setMotorSyncEncoder( driveMotors.motorNum[0].port, driveMotors.motorNum[1].port, 0, 999999, driveMotors.direction * tempPower * driveMotors.orientation);
//motor[driveMotors.motorNum[0].port] = driveMotors.direction * tempPower * driveMotors.orientation;
//motor[driveMotors.motorNum[1].port] = driveMotors.direction * tempPower * driveMotors.orientation;
// writeDebugStreamLine( "\ndriveMotorPower = %d;", driveMotors.direction * tempPower * driveMotors.orientation);
// writeDebugStreamLine( "\nMotor[0] = %d; Motor[1] = %d;", motor[driveMotors.motorNum[0].port], motor[driveMotors.motorNum[1].port]);
resetMotorStallChecker();
//nxtDisplayTextLine(4, "PwrLvl: %d %d", tempPower, motor[driveMotors.master]); // Just a debug message
}
void beginFollowingWall(int thisSide, int tempPower) {
if(menu.returnNow) return;
if(driveMotors.direction == STOPPED_MOVING) beginDrivingForward(tempPower);
IsFollowingLine = false; // Just driving forward is not following any line
synchMotors(driveMotors.master); // Only the master motor needs power since the other moter is synched to the master
driveMotors.turnRatio = 97; // One Motor will be 90% the value of the other
driveMotors.tempPowerLevel = tempPower;
motor[driveMotors.motorNum[0].port] = driveMotors.direction * tempPower * driveMotors.orientation;
motor[driveMotors.motorNum[1].port] = driveMotors.direction * tempPower * driveMotors.orientation * 0.97;
}
//////////////////////////////////////////////////////////////////////
//
// 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) {
if(menu.returnNow) return;
float turningCircumferenceMM = (2.0 * (float) config.centerOfWheelToCenterOfRobotMM * (float) PI);
float wheelRotationsPerRobotTurn = (float) turningCircumferenceMM / (float) config.wheelCircumferenceMM;
float wheelDegreesOfTurningNeeded = wheelRotationsPerRobotTurn * (float) turnDegrees;
float targetDegrees = (float) wheelDegreesOfTurningNeeded * (float) config.gearRatio;
float encoderTarget = (float) targetDegrees;
float followerTargetDegrees = targetDegrees;
long rotationSensor = 0;
long followerRotationSensor = 0;
float diffDegrees;
clearDisplay();
resetMotorStallChecker();
display(2," PointTurn");
display(3," Clockwise");
displayBigTextLine(1, "target %5d", (long) targetDegrees);
/*------
writeDebugStreamLine("Point Turn Clockwise");
writeDebugStreamLine("Degrees for Robot: %5f", (float) turnDegrees);
writeDebugStreamLine("Cntr Wheel to Bot: %5f", (float) config.centerOfWheelToCenterOfRobotMM);
writeDebugStreamLine("Turn Circumf: %5f", (float) turningCircumferenceMM);
writeDebugStreamLine("Wheel Rot per Trn: %5f", (float) wheelRotationsPerRobotTurn);
writeDebugStreamLine("Degrees Needed: %5f", (float) wheelDegreesOfTurningNeeded);
writeDebugStreamLine("Target Degrees: %5f", (float) targetDegrees);
-----*/
targetDegrees = targetDegrees + (float) nMotorEncoder[driveMotors.master];
followerTargetDegrees = (float) nMotorEncoder[driveMotors.slave] - followerTargetDegrees;
checkForEncoderError(targetDegrees); // Setting a MotorEncoder to an invalid number may result in an error. Better check first.
//synchMotors(driveMotors.master);
if(keepGoing) {
bFloatDuringInactiveMotorPWM = true; // coast or float
} else {
bFloatDuringInactiveMotorPWM = false;// brake
}
driveMotors.turnRatio = -100; // motors move in opposite directions of one another
driveMotors.tempPowerLevel = tempPower;
motor[driveMotors.master] = tempPower; // turns the motor on at specified power
motor[driveMotors.slave] = tempPower * -1; // turns the motor on at specified power
// The SetMotorSyncEncoder function doesn't seem to be able to turn the robot.
//setMotorSyncEncoder( driveMotors.motorNum[0].port, driveMotors.motorNum[1].port, -100, encoderTarget, driveMotors.direction * driveMotors.tempPowerLevel * driveMotors.orientation);
while(true) {
if(menu.returnNow) break;
if(driveMotors.motorNum[0].stalled || driveMotors.motorNum[1].stalled) { break; }
rotationSensor = nMotorEncoder[driveMotors.master]; // Current Rotation Sensor Value
followerRotationSensor = nMotorEncoder[driveMotors.slave]; // Current Rotation Sensor Value
if(rotationSensor > targetDegrees) motor[driveMotors.master] = 0;
if(followerRotationSensor < followerTargetDegrees) motor[driveMotors.slave] = 0;
if(rotationSensor > targetDegrees && followerRotationSensor < followerTargetDegrees) break;
diffDegrees = (float) ((targetDegrees - rotationSensor) - (followerRotationSensor - followerTargetDegrees));
if(motor[driveMotors.master] > 10) { // If there is power to the Master motor, then that means we haven't reached the
// target value and therefore we may still be able to make minor adjustments as needed.
// If the master's motor is less than 10, then don't try to make these minor adjustments
if(diffDegrees < -10.0) {
motor[driveMotors.slave] = (motor[driveMotors.master] + 8) * -1;
} else { if(diffDegrees < -5.0) {
motor[driveMotors.slave] = (motor[driveMotors.master] + 4) * -1;
} else { if(diffDegrees > 5.0) {
motor[driveMotors.slave] = (motor[driveMotors.master] - 4) * -1;
} else { if(diffDegrees > 10.0) {
motor[driveMotors.slave] = (motor[driveMotors.master] - 8) * -1;
} else {
motor[driveMotors.slave] = motor[driveMotors.master] * -1;
}}}}
}
#ifdef DEBUGSTREAM_MESSAGES_FOR_TURNING
// displayBigTextLine(8, "mE %5d>%5d", rotationSensor, (long) targetDegrees);
// displayBigTextLine(10,"fE %5d>%5d", followerRotationSensor, (long) followerTargetDegrees);
#endif
}
if(!keepGoing || menu.returnNow) stopMoving();
bFloatDuringInactiveMotorPWM = false; // brake
IsFollowingLine = false; // Turning means you are not following any line
}
void swingTurnClockwise(float turnDegrees, int tempPower) {
if(menu.returnNow) return;
float turningCircumferenceMM = (4 * config.centerOfWheelToCenterOfRobotMM * PI);
float wheelRotationsPerRobotTurn = turningCircumferenceMM / config.wheelCircumferenceMM;
float wheelDegreesOfTurningNeeded = wheelRotationsPerRobotTurn * turnDegrees;
float targetDegrees = wheelDegreesOfTurningNeeded * config.gearRatio;
clearDisplay();
resetMotorStallChecker();
sayWithStream(2," Swing Turn");
displayWithStream(3," Clockwise");
if(driveMotors.direction == STOPPED_MOVING) beginDrivingForward(driveMotors.powerLevel);
driveMotors.synched = false; // de-synch the motors
if(keepGoing) {
bFloatDuringInactiveMotorPWM = true; // coast or float
} else {
bFloatDuringInactiveMotorPWM = false;// brake
}
if(driveMotors.orientation == FORWARD) {
displayWithStream(5,"* Forward *");
targetDegrees = nMotorEncoder[driveMotors.master] + targetDegrees;
#ifdef DEBUGSTREAM_MESSAGES_FOR_TURNING
// writeDebugStreamLine("* MasterMotor: %d *", driveMotors.master);
// writeDebugStreamLine("* targetDegrees: %d *", targetDegrees);
#endif
checkForEncoderError(targetDegrees); // Setting a MotorEncoder target to an invalid number may result in a logic error. Better check first.
motor[driveMotors.slave] = 0; // turns the motor on at specified power
motor[driveMotors.master] = tempPower; // turns the motor on at specified power
driveMotors.tempPowerLevel = tempPower;
while(nMotorEncoder[driveMotors.master] <= targetDegrees) {
if(menu.returnNow) break;
if(driveMotors.motorNum[0].stalled) { break; } // motorNum[0] is the Master
writeDebugStreamLine("Target: %d Curr Deg: %d", targetDegrees, nMotorEncoder[driveMotors.master]); // Just a debug message
//nxtDisplayTextLine(5, "Target: %d", targetDegrees); // Just a debug message
//nxtDisplayTextLine(6, "Curr Deg: %d", nMotorEncoder[driveMotors.master]); // Just a debug message
}
if(!keepGoing || menu.returnNow) stopMoving();
} else {
targetDegrees = nMotorEncoder[driveMotors.slave] - targetDegrees;
checkForEncoderError(targetDegrees); // Setting a MotorEncoder target to an invalid number may result in a logic error. Better check first.
motor[driveMotors.master] = 0; // turns the motor on at specified power
motor[driveMotors.slave] = tempPower * -1; // turns the motor on at specified power
driveMotors.tempPowerLevel = tempPower * -1;
while(nMotorEncoder[driveMotors.slave] >= targetDegrees) {
if(menu.returnNow) break;
if(driveMotors.motorNum[1].stalled) { break; } // motorNum[1] is the Follower
#ifdef DEBUGSTREAM_MESSAGES_FOR_TURNING
// writeDebugStreamLine("Target: %d Curr Deg: %d", targetDegrees, nMotorEncoder[driveMotors.slave]); // Just a debug message
#endif
}
if(!keepGoing || menu.returnNow) stopMoving();
}
bFloatDuringInactiveMotorPWM = false; // brake
IsFollowingLine = false; // Turning means you are not following any line
}
void gradualTurnClockwise(float turnDegrees, float turnRadiusIN, int tempPower) {
if(menu.returnNow) return;
float innerTurnRadius = (turnRadiusIN * 25.4) - config.centerOfWheelToCenterOfRobotMM;
// Convert inches to millimeters by multiplying by 25.4
if(innerTurnRadius < 0) { innerTurnRadius = 0; }
float outerTurnRadius = (turnRadiusIN * 25.4) + config.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
}
resetMotorStallChecker();
sayWithStream(6," Gradual Turn *"); // Clear the message
displayWithStream(7,"* Clockwise *");
turnRatio = 100 - (innerTurnRadius / outerTurnRadius * 100.0);
writeDebugStreamLine( "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(driveMotors.direction == STOPPED_MOVING) beginDrivingForward(tempPower);
if(driveMotors.direction == FORWARD) {
targetDegrees = nMotorEncoder[driveMotors.master] + targetDegrees;
checkForEncoderError(targetDegrees); // Setting a MotorEncoder target to an invalid number may result in a logic error. Better check first.
setMotorSyncEncoder( driveMotors.master, driveMotors.slave, turnRatio, 999999, tempPower);
driveMotors.tempPowerLevel = tempPower;
while(nMotorEncoder[driveMotors.master] <= targetDegrees) {
if(menu.returnNow) break;
} //continue until the Target position is reached
if(!keepGoing) stopMoving();
} else {
targetDegrees = nMotorEncoder[driveMotors.slave] - targetDegrees;
checkForEncoderError(targetDegrees); // Setting a MotorEncoder target to an invalid number may result in a logic error. Better check first.
setMotorSyncEncoder( driveMotors.master, driveMotors.slave, -1 * turnRatio, 999999, -1 * tempPower);
driveMotors.tempPowerLevel = -1 * tempPower;
while(nMotorEncoder[driveMotors.slave] >= targetDegrees) {
if(menu.returnNow) break;
} //continue until the Target position is reached
if(!keepGoing) stopMoving();
}
bFloatDuringInactiveMotorPWM = false; // brake
IsFollowingLine = false; // Turning means you are not following any line
}
void pointTurnCounterClockwise(float turnDegrees, int tempPower) {
while(robotPaused) {}
if(menu.returnNow) return;
float turningCircumferenceMM = (2 * config.centerOfWheelToCenterOfRobotMM * PI);
float wheelRotationsPerRobotTurn = turningCircumferenceMM / config.wheelCircumferenceMM;
float wheelDegreesOfTurningNeeded = wheelRotationsPerRobotTurn * turnDegrees;
float targetDegrees = wheelDegreesOfTurningNeeded * config.gearRatio;
float followerTargetDegrees = targetDegrees;
long rotationSensor = 0;
long followerRotationSensor = 0;
float diffDegrees;
resetMotorStallChecker();
clearDisplay();
display(2," PointTurn");
display(3,"CounterClockwise");
targetDegrees = (float) nMotorEncoder[driveMotors.slave] + targetDegrees;
followerTargetDegrees = (float) nMotorEncoder[driveMotors.master] - followerTargetDegrees;
checkForEncoderError(targetDegrees); // Setting a MotorEncoder target to an invalid number may result in a logic error. Better check first.
//synchMotors(driveMotors.slave, -100.0);
if(keepGoing) {
bFloatDuringInactiveMotorPWM = true; // coast or float
} else {
bFloatDuringInactiveMotorPWM = false;// brake
}
motor[driveMotors.slave] = tempPower; //turns the motor on at specified power
motor[driveMotors.master] = tempPower * -1; //turns the motor on at specified power
while(true) {
if(menu.returnNow) break;
if(driveMotors.motorNum[0].stalled || driveMotors.motorNum[1].stalled) { break; }
rotationSensor = nMotorEncoder[driveMotors.slave]; // Current Rotation Sensor Value
followerRotationSensor = nMotorEncoder[driveMotors.master]; // Current Rotation Sensor Value
if(rotationSensor > targetDegrees) motor[driveMotors.slave] = 0;
if(followerRotationSensor < followerTargetDegrees) motor[driveMotors.master] = 0;
if(rotationSensor > targetDegrees && followerRotationSensor < followerTargetDegrees) break;
diffDegrees = (float) ((targetDegrees - rotationSensor) - (followerRotationSensor - followerTargetDegrees));
if(motor[driveMotors.slave] > 10) { // If there is power to the Master motor, then that means we haven't reached the
// target value and therefore we may still be able to make minor adjustments as needed.
// If the master's motor is less than 10, then don't try to make these minor adjustments
if(diffDegrees < -10.0) {
motor[driveMotors.master] = (motor[driveMotors.slave] + 8) * -1;
} else { if(diffDegrees < -5.0) {
motor[driveMotors.master] = (motor[driveMotors.slave] + 4) * -1;
} else { if(diffDegrees > 5.0) {
motor[driveMotors.master] = (motor[driveMotors.slave] - 4) * -1;
} else { if(diffDegrees > 10.0) {
motor[driveMotors.master] = (motor[driveMotors.slave] - 8) * -1;
} else {
motor[driveMotors.master] = motor[driveMotors.slave] * -1;
}}}}
}
#ifdef DEBUGSTREAM_MESSAGES_FOR_TURNING
// displayBigTextLine(8, "mE %5d>%5d", rotationSensor, (long) targetDegrees);
// displayBigTextLine(10,"fE %5d>%5d", followerRotationSensor, (long) followerTargetDegrees);
#endif
}
//continue to power motorC until the motor nMotorEncoderTarget position is reached
if(!keepGoing || menu.returnNow) stopMoving();
bFloatDuringInactiveMotorPWM = false; // brake
IsFollowingLine = false; // Turning means you are not following any line
}
void swingTurnCounterClockwise(float turnDegrees, int tempPower) {
while(robotPaused) {}
if(menu.returnNow) return;
float turningCircumferenceMM = (4 * config.centerOfWheelToCenterOfRobotMM * PI);
float wheelRotationsPerRobotTurn = turningCircumferenceMM / config.wheelCircumferenceMM;
float wheelDegreesOfTurningNeeded = wheelRotationsPerRobotTurn * turnDegrees;
float targetDegrees = wheelDegreesOfTurningNeeded * config.gearRatio;
clearDisplay();
resetMotorStallChecker();
display(2," SwingTurn");
display(3,"CounterClockwise");
if(driveMotors.direction == STOPPED_MOVING) beginDrivingForward(driveMotors.powerLevel);
driveMotors.synched = false; // de-synch the motors
if(keepGoing) {
bFloatDuringInactiveMotorPWM = true; // coast or float
} else {
bFloatDuringInactiveMotorPWM = false;// brake
}
if(driveMotors.orientation == FORWARD) {
display(5,"* Forward *");
writeDebugStreamLine("* SlaveMotor: %d *", driveMotors.slave);
targetDegrees = nMotorEncoder[driveMotors.slave] + targetDegrees;
writeDebugStreamLine("* targetDegrees: %d *", targetDegrees);
checkForEncoderError(targetDegrees); // Setting a MotorEncoder target to an invalid number may result in a logic error. Better check first.
motor[driveMotors.master] = 0; // turns the motor on at specified power
motor[driveMotors.slave] = tempPower; // turns the motor on at specified power
driveMotors.tempPowerLevel = tempPower;
while(nMotorEncoder[driveMotors.slave] <= targetDegrees) {
if(menu.returnNow) break;
if(driveMotors.motorNum[1].stalled) { break; } // motorNum[1] is the Follower
writeDebugStreamLine("Target: %d Curr Deg: %d", targetDegrees, nMotorEncoder[driveMotors.slave]); // Just a debug message
//nxtDisplayTextLine(5, "Target: %d", targetDegrees); // Just a debug message
//nxtDisplayTextLine(6, "Curr Deg: %d", nMotorEncoder[driveMotors.slave]); // Just a debug message
}
if(!keepGoing || menu.returnNow) stopMoving();
} else {
targetDegrees = nMotorEncoder[driveMotors.master] - targetDegrees;
checkForEncoderError(targetDegrees); // Setting a MotorEncoder target to an invalid number may result in a logic error. Better check first.
motor[driveMotors.slave] = 0; // turns the motor on at specified power
motor[driveMotors.master] = tempPower * -1; // turns the motor on at specified power
driveMotors.tempPowerLevel = tempPower * -1;
while(nMotorEncoder[driveMotors.master] >= targetDegrees) {
if(menu.returnNow) break;
if(driveMotors.motorNum[0].stalled) { break; } // motorNum[0] is the Master
#ifdef DEBUGSTREAM_MESSAGES_FOR_TURNING
// writeDebugStreamLine("Target: %d Curr Deg: %d", targetDegrees, nMotorEncoder[driveMotors.master]); // Just a debug message
#endif
//nxtDisplayTextLine(5, "Target: %d", targetDegrees); // Just a debug message
//nxtDisplayTextLine(6, "Curr Deg: %d", nMotorEncoder[driveMotors.master]); // Just a debug message
}
if(!keepGoing || menu.returnNow) stopMoving();
}
bFloatDuringInactiveMotorPWM = false; // brake
IsFollowingLine = false; // Turning means you are not following any line
}
void gradualTurnCounterClockwise(float turnDegrees, float turnRadiusIN, int tempPower) {
while(robotPaused) {}
if(menu.returnNow) return;
float innerTurnRadius = (turnRadiusIN * 25.4) - config.centerOfWheelToCenterOfRobotMM;
// Convert inches to millimeters by multiplying by 25.4
if(innerTurnRadius < 0) { innerTurnRadius = 0; }
float outerTurnRadius = (turnRadiusIN * 25.4) + config.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
}
resetMotorStallChecker();
display(6," GradualTurn *"); // Clear the message
display(7,"*CounterClockwise");
turnRatio = 100 - (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(driveMotors.direction == STOPPED_MOVING) beginDrivingForward(tempPower);
if(driveMotors.direction * driveMotors.orientation == FORWARD) {
targetDegrees = nMotorEncoder[driveMotors.slave] + targetDegrees;
checkForEncoderError(targetDegrees); // Setting a MotorEncoder target to an invalid number may result in a logic error. Better check first.
setMotorSyncEncoder( driveMotors.master, driveMotors.slave, -1 * turnRatio, 999999, tempPower);
while(nMotorEncoder[driveMotors.slave] <= targetDegrees) {
if(menu.returnNow) break;
if(driveMotors.motorNum[0].stalled || driveMotors.motorNum[1].stalled) { break; } // motorNum[0] is the Master
} // until the Target position is reached
if(!keepGoing || menu.returnNow) stopMoving();
} else {
targetDegrees = nMotorEncoder[driveMotors.master] - targetDegrees;
checkForEncoderError(targetDegrees); // Setting a MotorEncoder target to an invalid number may result in a logic error. Better check first.
setMotorSyncEncoder( driveMotors.master, driveMotors.slave, turnRatio, 999999, -1 * tempPower);
while(nMotorEncoder[driveMotors.master] >= targetDegrees) {
if(menu.returnNow) break;
} // until the motor Target position is reached
if(!keepGoing || menu.returnNow) stopMoving();
}
bFloatDuringInactiveMotorPWM = false; // brake
IsFollowingLine = false; // Turning means you are not following any line
}
/*---------------------------------------------------------
The Motor() function is used mostly to move the accesory motor(s) to specific degree values entered as the targetDegrees.
The targetDegree must be a number from 0 to 360. The motor encoder value is modulused with 360 and compared to the target.
If the absolute value of the difference between them is less than 5, then stop the motor (you're at the right spot).
--------------------------------------------------------*/
void Motor(int thisMotor, float targetDegrees, int dirRotation) {
float modEncoder;
while(robotPaused) {}
if(menu.returnNow) return;
motor[accessoryMotors.motorNum[thisMotor].port] = 0;
setMotorBrakeMode(accessoryMotors.motorNum[thisMotor].port, motorBrake); // Enable Braking
writeDebugStreamLine("===============\nStarting Motor()\nTo Degree: %4.1f\nThis Motor: %4d\nis curr at: %4d\n", targetDegrees, thisMotor, nMotorEncoder[accessoryMotors.motorNum[thisMotor].port]); // Just a debug message
sleep(200);
driveMotors.synched = false; // No motor synch
modEncoder = abs((nMotorEncoder[accessoryMotors.motorNum[thisMotor].port] % 360) - targetDegrees);
// writeDebugStreamLine(" 5) {
displayBigTextLine(12, "Motor: %4d", nMotorEncoder[accessoryMotors.motorNum[thisMotor].port] % 360);
// writeDebugStreamLine(" 5) {
displayBigTextLine(12, "Motor: %4d", nMotorEncoder[accessoryMotors.motorNum[thisMotor].port] % 360);
// writeDebugStreamLine(">Target: %d Curr Deg: %d mod: %d Diff: %d", targetDegrees, nMotorEncoder[accessoryMotors.motorNum[thisMotor].port], (nMotorEncoder[accessoryMotors.motorNum[thisMotor].port] % 360), modEncoder); // Just a debug message
modEncoder = abs((nMotorEncoder[accessoryMotors.motorNum[thisMotor].port] % 360) - targetDegrees);
displayBigTextLine(10, ">ModEnc: %4d", modEncoder);
}
}
motor[accessoryMotors.motorNum[thisMotor].port] = 0; // turns the motor on at specified power
sleep(100); // Pause a little to make sure motor has stopped.
// Enable coast or float in case the attachment is stuck or needs to rest
setMotorBrakeMode(accessoryMotors.motorNum[thisMotor].port, motorCoast);
motor[accessoryMotors.motorNum[thisMotor].port] = 0;
writeDebugStreamLine("Ended at Degree: %d\n", nMotorEncoder[accessoryMotors.motorNum[thisMotor].port]); // Just a debug message
motorError[convertMotorToInt(thisMotor)] = nMotorEncoder[accessoryMotors.motorNum[thisMotor].port] - targetDegrees;
sleep(20);
}
// 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 followColoredLine(TLegoColors tempColor, Side thisSide, tSensors nDeviceIndex) {
while(robotPaused) {}
if(menu.returnNow) return;
writeDebugStreamLine("\n=================\nIn followColoredLine();");
lineFollower.port=nDeviceIndex;
lineFollower.criteria = 30;
lineFollower.color = tempColor;
lineFollower.sideOfLine = thisSide; // Store which side of the line has been selected. This will
// be needed in the Stop functions that call FollowLine()
if(driveMotors.direction == STOPPED_MOVING) { driveMotors.direction = FORWARD; } // If you were stopped, then let's go forward.
if(lfColorPercent() > 30) { // Ooops. May be off of the line.
// The percent drops to zero only when the sensor is on that exact color.
// Otherwise, it will return an average of the other colors.
writeDebugStreamLine("Off of the line");
if(thisSide == RIGHTSIDE) { // sweep clockwise
pointTurnClockwise(25,30);
while(lfColorPercent() > 30) {
pointTurnCounterClockwise(5,30);
}
} else { // sweep counter clockwise
pointTurnCounterClockwise(25,30);
while(lfColorPercent() > 30) {
pointTurnClockwise(5,30);
}
}
resetMotorStallChecker();
}
IsFollowingLine = true; // This variable is used for the While() loops in the Stop functions.
if(driveMotors.direction == STOPPED_MOVING) { driveMotors.direction = FORWARD; } // If you were stopped, then let's go forward.
FollowLinePID();
}
void followColoredLine(TLegoColors tempColor, Side thisSide) {
while(robotPaused) {}
if(menu.returnNow) return;
followColoredLine(tempColor, thisSide, colorSensor);
}
void followLine(Side thisSide) {
while(robotPaused) {}
if(menu.returnNow) return;
followColoredLine(colorBlack, thisSide, colorSensor);
}
//////////////////////////////////////////////////////////////////////
//
// Various Stop Functions
//
void loopUntilLine(int tempLightLevel) {
if(driveMotors.direction == STOPPED_MOVING) beginDrivingForward(driveMotors.powerLevel);
if(driveMotors.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(lfColorPercent() > tempLightLevel) {
if(menu.returnNow) break;
if(driveMotors.motorNum[0].stalled || driveMotors.motorNum[1].stalled) { break; }
}
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'
}
void loopUntilColor(TLegoColors tempColor, tSensors nDeviceIndex, int tCriteria) {
if(menu.returnNow) return;
displayWithStream(3,"===============");
writeDebugStreamLine("Looking for color: %d", (int) tempColor);
long RedChannel;
long GreenChannel;
long BlueChannel;
// Purge the sensor of any bad data and get it ready
// When the sensor is first turned on, there are sometimes some erratic readings
// I guess there is some minute amount of time that is needed to adjust the sensor values
// The following lines may look unusual. But they serve the purpose of warming up or priming
// the sensor before it is used in the loop. Without this "warm-up" the first few
// sensor readings may be quite extreme and random in their appearance.
writeDebugStreamLine("Begin DoubleCheck");
int doubleCheck;
for(doubleCheck=0; doubleCheck<2; doubleCheck++) { // Better double-check that color.
sleep(200);
getColorRGB(nDeviceIndex, RedChannel, GreenChannel, BlueChannel);
getColorRGB(nDeviceIndex, RedChannel, GreenChannel, BlueChannel);
getColorRGB(nDeviceIndex, RedChannel, GreenChannel, BlueChannel);
getColorRGB(nDeviceIndex, RedChannel, GreenChannel, BlueChannel);
getColorRGB(nDeviceIndex, RedChannel, GreenChannel, BlueChannel);
switch(tempColor) {
case colorRed:
displayWithStream(2,"Looking for: Red");
while(true) {
if(menu.returnNow) break;
if(driveMotors.motorNum[0].stalled || driveMotors.motorNum[1].stalled) { break; }
if(IsFollowingLine) { FollowLinePID(); }
getColorRGB(nDeviceIndex, RedChannel, GreenChannel, BlueChannel);
if(RedChannel>tCriteria && GreenChanneltCriteria && BlueChannel50 && GreenChannel>50 && BlueChannel>50) break;
displayColorChannels( RedChannel, GreenChannel, BlueChannel);
}
break;
}
}
writeDebugStreamLine("\n*End of RGB *\n\n");
}
void untilLine(int tempLightLevel) {
loopUntilColor(colorBlack, colorSensor, tempLightLevel);
}
void untilLine() {
loopUntilColor(colorBlack, colorSensor, 30);
}
void untilColor(TLegoColors tempColor) {
loopUntilColor(tempColor, colorSensor, 30);
}
void stopOnLine() { untilLine(); stopMoving(); }
void stopOnLine(int tempLightLevel) { untilLine(tempLightLevel); stopMoving(); }
//void stopOnColor2(TLegoColors tempColor) { untilColor(tempColor); stopMoving(); }
void stopOnColor(TLegoColors tempColor, tSensors nDeviceIndex) {
if(menu.returnNow) return;
clearDisplay(); sayPhraseDisplay(1," stopOnColor");
loopUntilColor(tempColor, nDeviceIndex, 30);
stopMoving();
}
void stopOnColor(TLegoColors tempColor) {
stopOnColor(tempColor, colorSensor);
}
void stopOnColorWithTwoSensors(TLegoColors tempColor) {
if(menu.returnNow) return;
clearDisplay(); displayWithStream(1," stopOnColor");
display(3,"--------S1--S2");
long RedChannel1, GreenChannel1, BlueChannel1;
long RedChannel2, GreenChannel2, BlueChannel2;
// Purge the sensor of any bad data and get it ready
// When the sensor is first turned on, there are sometimes some erratic readings
// I guess there is some minute amount of time that is needed to adjust the sensor values
// The following lines may look unusual. But they serve the purpose of warming up or priming
// the sensor before it is used in the loop. Without this "warm-up" the first few
// sensor readings may be quite extreme and random in their appearance.
getColorRGB(colorSensor, RedChannel1, GreenChannel1, BlueChannel1);
getColorRGB(colorSensor2, RedChannel2, GreenChannel2, BlueChannel2);
getColorRGB(colorSensor, RedChannel1, GreenChannel1, BlueChannel1);
getColorRGB(colorSensor2, RedChannel2, GreenChannel2, BlueChannel2);
getColorRGB(colorSensor, RedChannel1, GreenChannel1, BlueChannel1);
getColorRGB(colorSensor2, RedChannel2, GreenChannel2, BlueChannel2);
getColorRGB(colorSensor, RedChannel1, GreenChannel1, BlueChannel1);
getColorRGB(colorSensor2, RedChannel2, GreenChannel2, BlueChannel2);
switch(tempColor) {
case colorRed:
displayWithStream(2,"Looking for: Red");
while(true) {
if(menu.returnNow) break;
if(driveMotors.motorNum[0].stalled || driveMotors.motorNum[1].stalled) { break; }
getColorRGB(colorSensor, RedChannel1, GreenChannel1, BlueChannel1);
getColorRGB(colorSensor2, RedChannel2, GreenChannel2, BlueChannel2);
if(RedChannel1>30 && GreenChannel1<30 && BlueChannel1<30) motor[motorB]=0;
if(RedChannel2>30 && GreenChannel2<30 && BlueChannel2<30) motor[motorC]=0;
if(RedChannel1>30 && GreenChannel1<30 && BlueChannel1<30 && RedChannel2>30 && GreenChannel2<30 && BlueChannel2<30) break;
writeDebugStreamLine("* 1: RGB: %3d %3d %3d *", RedChannel1, GreenChannel1, BlueChannel1);
writeDebugStreamLine("* 2: RGB: %3d %3d %3d *", RedChannel2, GreenChannel2, BlueChannel2);
displayBigTextLine(8, "Red: %3d %3d", RedChannel1, RedChannel2);
displayBigTextLine(10,"Green: %3d %3d", GreenChannel1, GreenChannel2);
displayBigTextLine(12,"Blue: %3d %3d", BlueChannel1, BlueChannel2);
}
break;
case colorBlack:
displayWithStream(2,"Looking for: Black");
while(true) {
if(menu.returnNow) break;
if(driveMotors.motorNum[0].stalled || driveMotors.motorNum[1].stalled) { break; }
getColorRGB(colorSensor, RedChannel1, GreenChannel1, BlueChannel1);
getColorRGB(colorSensor2, RedChannel2, GreenChannel2, BlueChannel2);
if(RedChannel1<30 && GreenChannel1<30 && BlueChannel1<30) motor[motorB]=0;
if(RedChannel2<30 && GreenChannel2<30 && BlueChannel2<30) motor[motorC]=0;
if(RedChannel1<30 && GreenChannel1<30 && BlueChannel1<30 && RedChannel2<30 && GreenChannel2<30 && BlueChannel2<30) break;
writeDebugStreamLine("* 1: RGB: %3d %3d %3d *", RedChannel1, GreenChannel1, BlueChannel1);
writeDebugStreamLine("* 2: RGB: %3d %3d %3d *", RedChannel2, GreenChannel2, BlueChannel2);
// displayBigTextLine(8, "Red: %3d %3d", RedChannel1, RedChannel2);
// displayBigTextLine(10,"Green: %3d %3d", GreenChannel1, GreenChannel2);
// displayBigTextLine(12,"Blue: %3d %3d", BlueChannel1, BlueChannel2);
}
break;
case colorGreen:
displayWithStream(2,"Looking for: Green");
while(true) {
if(menu.returnNow) break;
if(driveMotors.motorNum[0].stalled || driveMotors.motorNum[1].stalled) { break; }
getColorRGB(colorSensor, RedChannel1, GreenChannel1, BlueChannel1);
getColorRGB(colorSensor2, RedChannel2, GreenChannel2, BlueChannel2);
if(RedChannel1<30 && GreenChannel1>30 && BlueChannel1<30 && motor[motorB]>0) { motor[motorB]=0; writeDebugStreamLine("* 1: RGB: %3d %3d %3d *", RedChannel1, GreenChannel1, BlueChannel1); writeDebugStreamLine("Criteria met on 1"); }
if(RedChannel2<30 && GreenChannel2>30 && BlueChannel2<30 && motor[motorC]>0) { motor[motorC]=0; writeDebugStreamLine("* 2: RGB: %3d %3d %3d *", RedChannel2, GreenChannel2, BlueChannel2); writeDebugStreamLine("Criteria met on 2"); }
if(RedChannel1<30 && GreenChannel1>30 && BlueChannel1<30 && RedChannel2<30 && GreenChannel2>30 && BlueChannel2<30) break;
writeDebugStreamLine("* 1: RGB: %3d %3d %3d *", RedChannel1, GreenChannel1, BlueChannel1);
writeDebugStreamLine("* 2: RGB: %3d %3d %3d *", RedChannel2, GreenChannel2, BlueChannel2);
// displayBigTextLine(8, "Red: %3d %3d", RedChannel1, RedChannel2);
// displayBigTextLine(10,"Green: %3d %3d", GreenChannel1, GreenChannel2);
// displayBigTextLine(12,"Blue: %3d %3d", BlueChannel1, BlueChannel2);
}
break;
}
displayBigTextLine(8, "Red: %3d %3d", RedChannel1, RedChannel2);
displayBigTextLine(10,"Green: %3d %3d", GreenChannel1, GreenChannel2);
displayBigTextLine(12,"Blue: %3d %3d", BlueChannel1, BlueChannel2);
writeDebugStreamLine("\n*End of RGB *\n\n");
stopMoving();
}
void untilSonarLessThan(float distance_in_inches) {
while(robotPaused) {}
if(menu.returnNow) return;
if(distance_in_inches <=3) {
displayWithStream(0,"*****************");
sayWithStream(2,"*!! Warning !!*");
displayWithStream(3,"*Sonar Sensor not");
displayWithStream(4,"*able to detect *");
displayWithStream(5,"*distance <4 inch");
displayWithStream(6,"*Press Orange Btn");
displayWithStream(7,"*to continue. *");
waitForOrangeButton();
}
distance_in_inches = distance_in_inches + (float) ((float)driveMotors.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
displayWithStream(3," SONAR Checking *"); // Clear the message
displayWithStream(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) ultrasonicSensor] > distance_in_cm) {
if(menu.returnNow) break;
if(driveMotors.motorNum[0].stalled || driveMotors.motorNum[1].stalled) { break; }
//nxtDisplayTextLine(7, "Curr %d ", SensorValue[sonarSensor]);
if(driveMotors.direction == STOPPED_MOVING) beginDrivingForward(driveMotors.powerLevel);
if(IsFollowingLine) { FollowLinePID(); }
}
}
bool isSonarLessThan(float distance_in_inches) {
float distance_in_cm; distance_in_cm = distance_in_inches * 2.54;
return (SensorValue((short) ultrasonicSensor) < distance_in_cm);
}
bool isSonarGreaterThan(float distance_in_inches) {
float distance_in_cm; distance_in_cm = distance_in_inches * 2.54;
return (SensorValue((short) ultrasonicSensor) > distance_in_cm);
}
void stopIfSonarLessThan(float distance_in_inches) { untilSonarLessThan(distance_in_inches); stopMoving(); }
void untilSonarGreaterThan(float distance_in_inches) {
writeDebugStreamLine("Until Sonar is greater than: %6.3f", distance_in_inches); // Clear the message
while(robotPaused) {}
if(menu.returnNow) return;
float distance_in_cm; distance_in_cm = distance_in_inches * 2.54;
while(SensorValue(ultrasonicSensor) <= distance_in_cm) {
writeDebugStreamLine("Sonar is: %6.3f %6.3f", SensorValue(ultrasonicSensor), distance_in_cm); // Clear the message
if(menu.returnNow) break;
if(driveMotors.motorNum[0].stalled || driveMotors.motorNum[1].stalled) { break; }
if(IsFollowingLine) { FollowLinePID(); } // If going forward, then check rotation sensor
}
}
void untilDistance(float inches) {
while(robotPaused) {}
if(menu.returnNow) return;
displayBigTextLine(8, "Distance %4.1f", inches);
long rotationSensor = 0;
long followerRotationSensor = 0;
int currPowerLevel= motor[driveMotors.master];
float targetDegrees, encoderTarget, followerTargetDegrees, diffDegrees;
bool motorIsRunning;
rotationSensor = nMotorEncoder[driveMotors.master]; // Starting Rotation Sensor Value
followerRotationSensor = nMotorEncoder[driveMotors.slave]; // Starting Rotation Sensor Value
writeDebugStreamLine(" Until Dis: %3.3f", inches); // Clear the message
display(7," *");
// WheelCircumferenceMM is in millimeters, so, multiply inches by 25.4 to convert to mm.
writeDebugStreamLine( "--------------\nin untilDistance();");
if(driveMotors.direction == BACKWARD) {
writeDebugStreamLine( "Going Backward");
}
// writeDebugStreamLine( "WheelCirc: %6.2f", config.wheelCircumferenceMM);
// writeDebugStreamLine( "gearRatio: %6.2f", config.gearRatio);
// writeDebugStreamLine( "Inches: %6.2f", inches);
// writeDebugStreamLine( "RobotDirection: %d", driveMotors.direction);
// writeDebugStreamLine( "MotorOrientation: %d", driveMotors.orientation);
// writeDebugStreamLine( "MotorPower: %d",motor[motorB]);
if(IsFollowingLine) { writeDebugStreamLine( "IsFollowingLine"); } else { writeDebugStreamLine( "Is NOT FollowingLine"); }
targetDegrees = (float) 360 * (float) inches * 25.4 / (float) config.wheelCircumferenceMM * (float) config.gearRatio; // Num of Degrees to turn
encoderTarget = (float) targetDegrees;
followerTargetDegrees = targetDegrees;
bFloatDuringInactiveMotorPWM = false; // brake
targetDegrees *= driveMotors.direction * driveMotors.orientation;
followerTargetDegrees *= driveMotors.direction * driveMotors.orientation;
targetDegrees += rotationSensor; // Add these degrees to the current rotation sensor
followerTargetDegrees += followerRotationSensor; // 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[driveMotors.master] = targetDegrees; // This is the new target degree to achieve
if(driveMotors.direction == STOPPED_MOVING) beginDrivingForward(driveMotors.powerLevel);
//setMotorSyncEncoder( driveMotors.motorNum[0].port, driveMotors.motorNum[1].port, 0, encoderTarget, driveMotors.direction * driveMotors.tempPowerLevel * driveMotors.orientation);
if(currPowerLevel == 0) {
writeDebugStreamLine( "Motors are currently off.\nWill turn them on to: %d", driveMotors.powerLevel);
motor[driveMotors.master] = driveMotors.powerLevel; motor[driveMotors.slave] = driveMotors.powerLevel;
if(!IsFollowingLine) setMotorSyncEncoder( driveMotors.motorNum[0].port, driveMotors.motorNum[1].port, 0, 999999, driveMotors.direction * driveMotors.powerLevel * driveMotors.orientation);
}
// if(direction * mDir == FORWARD) { PlaySoundFile("Forward.rso");PlaySoundFile("Turn.rso"); }
// if(direction * mDir == BACKWARD) { PlaySoundFile("Backwards.rso"); }
writeDebugStreamLine( " Master Encoder: %d", nMotorEncoder[driveMotors.master]);
writeDebugStreamLine( " Master Target: %d", targetDegrees);
//writeDebugStreamLine( " Follower Encoder: %d", nMotorEncoder[driveMotors.slave]);
//writeDebugStreamLine( " Follower Target: %d", followerTargetDegrees);
while(true) {
if(menu.returnNow) break;
motorIsRunning = getMotorRunning(driveMotors.motorNum[0].port); // Get the status of the motor
writeDebugStreamLine("Looking at Motor: %d", driveMotors.motorNum[0].port );
rotationSensor = nMotorEncoder[driveMotors.master]; // Current Rotation Sensor Value
followerRotationSensor = nMotorEncoder[driveMotors.slave]; // Current Rotation Sensor Value
// The setMotorSyncEncoder function will set the target degrees to tell the robot when to stop.
// But it will also synch the motors which is not desired for line following.
if((driveMotors.direction * driveMotors.orientation)==FORWARD && rotationSensor >= (long) targetDegrees) break;
if((driveMotors.direction * driveMotors.orientation)==BACKWARD && rotationSensor <= (long) targetDegrees) break;
if(driveMotors.motorNum[0].stalled || driveMotors.motorNum[1].stalled) { writeDebugStreamLine( "Stalled!" ); break; }
//writeDebugStreamLine( "Encoder: %6d Trying to reach target: %6d", nMotorEncoder[driveMotors.master], targetDegrees);
displayBigTextLine(10, "mE %5d %5d", nMotorEncoder[driveMotors.master], rotationSensor);
displayBigTextLine(12, "mT %5d", targetDegrees);
if(IsFollowingLine) {
FollowLinePID();
} else {
// You can check the motorIsRunning variable if you are not following a line.
// If you are following a line, then there is a chance that one of the motors may be stopped while turning the robot
if(!motorIsRunning) { writeDebugStreamLine( "Not Running!" ); break; }
/*--------------------------------------------
If you are not following a line, then you are simply driving straight.
Getting the robot to go straight may be tricky without some sort of controller that
adjusts for error in the tracking (i.e., slight differences between the motors).
For example, if one motor spins at a slightly faster rate than the other, the robot will veer
to one side instead of driving straight. Thus it becomes important for the robot to
check to see how straight it is going by using a separate motor encoder target for each motor.
Comparing the current motor encoder value to the motor encoder target; then calculating the difference;
then finding the difference between these two motors, we can obtain a value which tells us which
way the robot is veering. If the difference is negative, it is veering to one side. If the
difference is positive, it is veering to the other side. The amount of the difference corresponds to the
amount the robot has veered. So, simply apply more or less power depending on the direction and amount
of the differences. This could be done using a PID controller. But in the example below, it is accomplished
using simple "if" blocks.
---------------------------------------------*/
#ifdef USE_MANUAL_SYNCHRONIZATION
diffDegrees = (float) ((targetDegrees - rotationSensor) - (followerTargetDegrees - followerRotationSensor));
if(diffDegrees < -10.0) {
motor[driveMotors.slave] = motor[driveMotors.master] + 8;
} else { if(diffDegrees < -5.0) {
motor[driveMotors.slave] = motor[driveMotors.master] + 4;
} else { if(diffDegrees > 5.0) {
motor[driveMotors.slave] = motor[driveMotors.master] - 4;
} else { if(diffDegrees > 10.0) {
motor[driveMotors.slave] = motor[driveMotors.master] - 8;
} else {
motor[driveMotors.slave] = motor[driveMotors.master];
}}}}
#endif
}
}
writeDebugStreamLine( "Target reached\n");
// PlaySoundFile("Bravo.rso");
}
void stopAfterDistance(float inches) { untilDistance(inches); stopMoving(); }
void untilRotations(float turnRotations) {
while(robotPaused) {}
if(menu.returnNow) return;
long rotationSensor = 0;
float targetDegrees;
displayBigTextLine(8, "Rotations %4.1f", turnRotations);
rotationSensor = nMotorEncoder[motorB];
// WheelCircumferenceMM is in millimeters, so, multiply inches by 25.4 to convert to mm.
targetDegrees = 360 * turnRotations; // Num of Degrees to turn
bFloatDuringInactiveMotorPWM = false; // brake
targetDegrees *= driveMotors.direction * driveMotors.orientation;
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
while(
(driveMotors.direction * driveMotors.orientation==FORWARD && nMotorEncoder[driveMotors.master] <= targetDegrees) ||
(driveMotors.direction * driveMotors.orientation==BACKWARD && nMotorEncoder[driveMotors.master] >= targetDegrees)) {
motor[driveMotors.master] = driveMotors.powerLevel;
writeDebugStreamLine( "Encoder: %6d Trying to reach target: %6d", nMotorEncoder[driveMotors.master], targetDegrees);
if(IsFollowingLine) { FollowLinePID(); }
}
/*----
if(driveMotors.direction == BACKWARD) { // If going backward, then check rotation sensor >= target
while(nMotorRunState[driveMotors.master] != runStateIdle && nMotorRunState[driveMotors.master] != runStateHoldPosition && nMotorEncoder[driveMotors.master] >= targetDegrees) {
if(IsFollowingLine) { FollowLinePID(); }
}
} else { // If going forkward, then check rotation sensor <= target
while(nMotorRunState[driveMotors.master] != runStateIdle && nMotorRunState[driveMotors.master] != runStateHoldPosition && nMotorEncoder[driveMotors.master] <= 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;
long followerRotationSensor = 0;
int currPowerLevel= 0;
bool motorIsRunning;
writeDebugStreamLine( "==============\nIn accelerateOverDistance();" );
writeDebugStreamLine( "Curr motor direction is: %d", driveMotors.direction );
if(driveMotors.direction == STOPPED_MOVING) beginDrivingForward(driveMotors.powerLevel);
currPowerLevel = motor[driveMotors.master];
if(currPowerLevel == 0) currPowerLevel = driveMotors.powerLevel;
setMotorSyncEncoder( driveMotors.motorNum[0].port, driveMotors.motorNum[1].port, 0, 999999, driveMotors.direction * currPowerLevel * driveMotors.orientation);
if(currPowerLevel >= power) return; // Can't accelerate. Already going that fast.
float powerIncrement = (float) (power-currPowerLevel) / 10;
float incrementAtDegree;
rotationSensor = nMotorEncoder[driveMotors.master];
// 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
incrementAtDegree = targetDegrees / 10.0;
bFloatDuringInactiveMotorPWM = false; // brake
// if(driveMotors.direction == BACKWARD) { targetDegrees*= -1; } // Direction is reverse if <0
writeDebugStreamLine( "Curr Rotation Sensor: %5d\nNumber of Degrees to Move: %5d\nTarget: %5d", rotationSensor, targetDegrees );
targetDegrees += rotationSensor; // Add these degrees to the current rotation sensor
// writeDebugStreamLine( "Target: %5d", targetDegrees );
checkForEncoderError(targetDegrees); // Setting a MotorEncoder to an invalid number may result in an error. Better check first.
writeDebugStreamLine( "Incrementing Speed at: %5.0f degrees", incrementAtDegree );
writeDebugStreamLine( "\ndriveMotorPower = %d;", currPowerLevel);
// writeDebugStreamLine( "Power for motors is currently:\nMotor[0] = %d; Motor[1] = %d;", motor[driveMotors.motorNum[0].port], motor[driveMotors.motorNum[1].port]);
//nMotorEncoderTarget[motorB] = targetDegrees; // This is the new target degree to achieve
while(true) {
if(menu.returnNow) break;
motorIsRunning = getMotorRunning(driveMotors.motorNum[0].port); // Get the status of the motor
rotationSensor = nMotorEncoder[driveMotors.master]; // Current Rotation Sensor Value
followerRotationSensor = nMotorEncoder[driveMotors.slave]; // Current Rotation Sensor Value
if((driveMotors.direction * driveMotors.orientation)==FORWARD && rotationSensor >= (long) targetDegrees) break;
if((driveMotors.direction * driveMotors.orientation)==BACKWARD && rotationSensor <= (long) targetDegrees) break;
if(driveMotors.motorNum[0].stalled || driveMotors.motorNum[1].stalled) { writeDebugStreamLine( "Stalled!" ); break; }
if(!motorIsRunning) { writeDebugStreamLine( "Not Running!" ); break; }
//writeDebugStreamLine( "Encoder: %6d Trying to reach target: %6d", nMotorEncoder[driveMotors.master], targetDegrees);
//displayBigTextLine(10, "mE %5d %5d", nMotorEncoder[driveMotors.master], rotationSensor);
//displayBigTextLine(12, "mT %5d", targetDegrees);
if(nMotorEncoder[motorB] > incrementAtDegree && currPowerLevel < power) {
incrementAtDegree+=36;
currPowerLevel += (driveMotors.direction * driveMotors.orientation * powerIncrement);
if(currPowerLevel > power) currPowerLevel = power;
setMotorSyncEncoder( driveMotors.motorNum[0].port, driveMotors.motorNum[1].port, 0, 999999, currPowerLevel);
}
if(IsFollowingLine) { FollowLinePID(); }
}
setPowerLevel(power); // Reset the default power level to the passed power
}
void stopAfterDistanceWithAcceleration(float inches, int power) { accelerateOverDistance(inches, power); stopMoving(); }
void untilTouch(tSensors nDeviceIndex) {
while(robotPaused) {}
display(4, "Until Touch");
// while(!SensorValue[touchSensor1]) { // Loop until the button is pressed
while(!SensorValue[nDeviceIndex]) { // Loop until the button is pressed
if(driveMotors.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 untilTouch() { untilTouch(touchSensor1); }
void stopOnTouch(tSensors nDeviceIndex) { untilTouch(nDeviceIndex); stopMoving(); }
void stopOnTouch() { untilTouch(touchSensor1); stopMoving(); }
void untilRelease() {
while(SensorValue[touchSensor1]) { // Loop until the button is pressed
}
}
//////////////////////////////////////////////////////////////////////
//
// Special Functions
//
float rotationsToDegrees(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(rotationsToDegrees(5)); moves accessory for 5 rotations
/*-------------
void align(int dirRotation) {
IsFollowingLine = false; // Just driving forward is not following any line
// nSyncedMotors = synchBC; // motor B is the master, motor C is the follower
SensorType[S2] = sensorLightActive; // Both sensors are used in this alignment function.
SensorType[S3] = sensorLightActive; // Make sure you turn the light 'on'
// Drive to the line
if(driveMotors.direction == STOPPED_MOVING) { driveMotors.direction = FORWARD; } // If you were stopped, then let's go forward.
if(driveMotors.direction == (FORWARD)) {
motor[driveMotors.master] = driveMotors.powerLevel;// Only the master motor needs power since the other moter is synched to the master
} else {
motor[driveMotors.master] = driveMotors.powerLevelRev;// Only the master motor needs power since the other moter is synched to the master
}
// Enter a loop and stay there as long as the light value is greater than % in variable lineIsAtLevel
while(getLightPercent() > lineIsAtLevel) { }
// Stop moving now. Prepare robot to turn in place.
//motor[motorB] = 0; // turn the motor off.
//motor[motorC] = 0; // turn the motor off.
float criteria1,criteria2,lightLevelPortion;
lightLevelPortion = (float) lineIsAtLevel / 100;
criteria1 = lightConfig1.lowLight + ((lightConfig1.highLight - lightConfig1.lowLight) * lightLevelPortion);
criteria2 = lightConfig2.lowLight + ((lightConfig2.highLight - lightConfig2.lowLight) * lightLevelPortion);
long point1, point2;
float targetDegrees;
point1 = nMotorEncoder[motorB]; // Get first rotation sensor reading
if(driveMotors.direction == FORWARD) {
motor[driveMotors.master] = driveMotors.powerLevel;// Only the master motor needs power since the other moter is synched to the master
while(SensorRaw[lightConfig2.port] > criteria2) { }
point2 = nMotorEncoder[driveMotors.master]; // Get second rotation sensor reading
targetDegrees = (point1 + point2) / 2; // Make the robot have a slight bias toward point2. This will
// place the robot in a better position for line following.
motor[driveMotors.master] = driveMotors.powerLevelRev;// Only the master motor needs power since the other moter is synched to the master
while(nMotorEncoder[driveMotors.master] >= targetDegrees) { }
} else {
motor[driveMotors.master] = driveMotors.powerLevelRev;// Only the master motor needs power since the other moter is synched to the master
while(SensorRaw[lightConfig1.port] > criteria1) { }
point2 = nMotorEncoder[driveMotors.master]; // Get second rotation sensor reading
targetDegrees = (point1 + point2) / 2; // Make the robot have a slight bias toward point2. This will
// place the robot in a better position for line following.
motor[driveMotors.master] = driveMotors.powerLevel;// Only the master motor needs power since the other moter is synched to the master
while(nMotorEncoder[driveMotors.master] <= targetDegrees) { }
}
// Prepare the robot to turn in place
motor[motorB] = 0; // turn the motor off.
motor[motorC] = 0; // turn the motor off.
// nSyncedTurnRatio = -100; //motors move in opposite directions of one another
if(dirRotation == COUNTERCLOCKWISE) {
motor[motorB] = driveMotors.powerLevelRev * .6; //turns the motor on at specified power
motor[motorC] = driveMotors.powerLevel * .6; //turns the motor on at specified power
} else {
motor[motorB] = driveMotors.powerLevel * .6; //turns the motor on at specified power
motor[motorC] = driveMotors.powerLevelRev * .6; //turns the motor on at specified power
}
if(driveMotors.direction == FORWARD) {
while(SensorRaw[lightConfig2.port] > criteria2) { }
} else {
while(SensorRaw[lightConfig1.port] > criteria1) { }
}
stopMoving();
// nSyncedMotors = synchNone; // de-synch the motors
}
--------*/
void accessoryToDegree(int tempMotor, float turnDegrees, int tempPower) {
clearDisplay(); display(2," Accessory Motor"); display(3," to Degree");
accessoryMotors.powerLevel = tempPower;
Motor(tempMotor,(float) turnDegrees, FORWARD);
}
void moveAccessoryUp(MotorNdxType motorNdx, float turnDegrees, int tempPower) {
bool motorIsRunning;
while(robotPaused) {}
if(menu.returnNow) return;
resetMotorStallChecker();
accessoryMotors.motorNum[0].stalled = false; // Reset the stall checker
accessoryMotors.motorNum[1].stalled = false;
long rotationSensor;
int currPort = accessoryMotors.motorNum[motorNdx].port;
resetMotorEncoder(currPort);
setMotorBrakeMode(currPort, motorBrake);
if(turnDegrees<0) turnDegrees=999999; // If no degrees are specified, then travel an infinite distance.
moveMotorTarget(currPort, turnDegrees, tempPower);
rotationSensor = nMotorEncoder[currPort];
//writeDebugStreamLine("In Move Accessory Up");
//writeDebugStreamLine("Rotation Sensor: %6d", rotationSensor);
//writeDebugStreamLine("Power for This Motor: %6d", tempPower);
//writeDebugStreamLine("Port for This Motor: %6d", currPort);
while(true) {
accessoryMotors.motorNum[0].speed = motor[accessoryMotors.motorNum[0].port];
accessoryMotors.motorNum[1].speed = motor[accessoryMotors.motorNum[1].port];
//writeDebugStreamLine("x motorSpeed: %d", motor[motorD] );
motorIsRunning = getMotorRunning(currPort); // Get the status of the motor
if(menu.returnNow) break;
if(accessoryMotors.motorNum[motorNdx].stalled) { break; }
// if(!accessoryMotors.motorNum[motorNdx].moving) { break; }
if(!motorIsRunning) { break; }
sleep(100);
}
motor[currPort] = 0;
sleep(100);
setMotorBrakeMode(currPort, motorCoast);
//bFloatDuringInactiveMotorPWM = false; // coast or float
// nMotorEncoder[currPort] = 0;
wait1Msec(20);
//writeDebugStreamLine("Bye");
accessoryMotors.motorNum[0].speed = motor[accessoryMotors.motorNum[0].port];
accessoryMotors.motorNum[1].speed = motor[accessoryMotors.motorNum[1].port];
}
void moveAccessoryDown(MotorNdxType motorNdx, float turnDegrees, int tempPower) {
bool motorIsRunning;
while(robotPaused) {}
if(menu.returnNow) return;
resetMotorStallChecker();
accessoryMotors.motorNum[0].stalled = false;
accessoryMotors.motorNum[1].stalled = false;
long rotationSensor;
//bFloatDuringInactiveMotorPWM = false; // Use the Brakes
int currPort = accessoryMotors.motorNum[motorNdx].port;
resetMotorEncoder(currPort);
setMotorBrakeMode(currPort, motorBrake);
if(turnDegrees<0) turnDegrees=999999; // If no degrees are specified, then travel an infinite distance.
moveMotorTarget(currPort, turnDegrees, -1.0 * tempPower);
//motor[currPort] = -1 * tempPower;
//setMotorTarget(currPort, turnDegrees * -1, tempPower);
rotationSensor = nMotorEncoder[currPort];
//writeDebugStreamLine("In Move Accessory Down");
//writeDebugStreamLine("Rotation Sensor: %6d", rotationSensor);
//writeDebugStreamLine("Power for This Motor: %6d", tempPower);
//writeDebugStreamLine("Port for This Motor: %6d", currPort);
while(true) {
accessoryMotors.motorNum[0].speed = (int) motor[accessoryMotors.motorNum[0].port];
accessoryMotors.motorNum[1].speed = (int) motor[accessoryMotors.motorNum[1].port];
writeDebugStreamLine("A motorSpeed: %d", (int) motor[motorA] + (int) motor[motorD] );
// writeDebugStreamLine("while Motor Speed 0 is: %3d on port: %d or S2: %d", (int) abs(accessoryMotors.motorNum[0].speed), accessoryMotors.motorNum[0].port, (int) getMotorSpeed(motorA));
// writeDebugStreamLine("while Motor Speed 1 is: %3d on port: %d or S2: %d", (int) abs(accessoryMotors.motorNum[1].speed), accessoryMotors.motorNum[1].port, (int) getMotorSpeed(motorD));
motorIsRunning = getMotorRunning(currPort); // Get the status of the motor
if(menu.returnNow) break;
if(accessoryMotors.motorNum[motorNdx].stalled) { break; }
writeDebugStreamLine("Is Stalled?: 0:%1d 1:%1d; curr runing: %1d; curr encoder: %d", (accessoryMotors.motorNum[0].stalled) ? 1: 0, (accessoryMotors.motorNum[1].stalled) ? 1: 0, (motorIsRunning) ? 1: 0, nMotorEncoder[currPort] );
if(!motorIsRunning) { break; }
sleep(100);
// if(abs(rotationSensor - nMotorEncoder[currPort]) < 5) { break; }
// rotationSensor = nMotorEncoder[currPort];
}
/*-------------
if(turnDegrees < 0) {
while(true) {
if(menu.returnNow) break;
sleep(100);
if(abs(rotationSensor - nMotorEncoder[currPort]) < 5) { break; }
rotationSensor = nMotorEncoder[currPort];
}
motor[currPort] = 0;
sleep(100);
motorError[convertMotorToInt(currPort)] = 0; // Clear the error now that the motor has stalled. Don't want this error affecting the next action.
} else {
float targetDegrees = ((nMotorEncoder[currPort] - turnDegrees) - motorError[convertMotorToInt(currPort)]);
writeDebugStreamLine("Target degrees: %6.0f", targetDegrees);
while(nMotorEncoder[currPort] >= targetDegrees + ((float)tempPower * .6)) {
if(accessoryMotors.motorNum[0].stalled || accessoryMotors.motorNum[1].stalled) { break; }
if(menu.returnNow) break;
writeDebugStreamLine("Moving");
}
motor[currPort] = 0;
sleep(100);
motorError[convertMotorToInt(currPort)] = nMotorEncoder[convertMotorToInt(currPort)] - targetDegrees;
}
bFloatDuringInactiveMotorPWM = false; // Keep Brake on
----*/
motor[currPort] = 0;
sleep(100);
setMotorBrakeMode(currPort, motorCoast);
wait1Msec(20);
writeDebugStreamLine("Bye");
accessoryMotors.motorNum[0].speed = motor[accessoryMotors.motorNum[0].port];
accessoryMotors.motorNum[1].speed = motor[accessoryMotors.motorNum[1].port];
}
void forwardToLine() {
while(robotPaused) {}
if(menu.returnNow) return;
/*---------
beginDrivingForward(driveMotors.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]=driveMotors.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]=driveMotors.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);
------*/
}
/*-------------------
This BackIntoWall function uses two touch sensors (one behind each wheel).
When one touch sensor is triggered, then that motor stops. Robot stops moving
when both touch sensors are triggered (i.e., the robot is touching the wall)
-------------------*/
void beginBackIntoWallUsingTouch(int tempPower) {
while(robotPaused) {}
if(menu.returnNow) return;
clearDisplay();
display(2, "Back Into Wall");
beginDrivingBackward(tempPower);
while(!SensorValue[touchSensor1] && !SensorValue[touchSensor2]) { // Loop until the button is pressed
if(driveMotors.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(driveMotors.direction == BACKWARD) { // If going forward, then check rotation sensor <= target
motor[motorC]=tempPower;
} else {
motor[motorC]=tempPower * -1;
}
}
stopMoving();
}
if(!SensorValue[touchSensor1]) {
while(!SensorValue[touchSensor1]) { // Loop until the button is pressed
// nSyncedMotors = synchNone; // No motor synch
bFloatDuringInactiveMotorPWM = false; // Enable braking
if(driveMotors.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 beginBackIntoWallUsingStall(int tempPower) {
while(robotPaused) {}
if(menu.returnNow) return;
beginDrivingBackward(30); // Use a low power level for this method so that the stall will occur easier
while(true) { // Only break from the loop if both motors are stalled.
if(driveMotors.motorNum[0].stalled && driveMotors.motorNum[1].stalled) { break; }
if(menu.returnNow) break;
}
}
void backIntoWall() {
beginBackIntoWallUsingStall(driveMotors.powerLevel);
}
void backIntoWall(int tempPower) {
beginBackIntoWallUsingStall(tempPower);
}
/////////////////////////////////////////////////////////////////////////////////////////////
//
// Define the Overloaded Functions
//
void pause() {
waitForTouchSensor();
}
void driveForward() { // Overloaded function: 0 parameters
while(robotPaused) {}
if(menu.returnNow) return;
sayPhraseDebugStream("In Drive Forward");
beginDrivingForward(driveMotors.powerLevel);
writeDebugStreamLine("Done with driveForward();");
}
void driveForward(float inches) { // Overloaded function: 1 parameter
while(robotPaused) {}
if(menu.returnNow) return;
//writeDebugStreamLine("In driveForward(distance);");
beginDrivingForward(driveMotors.powerLevel);
stopAfterDistance(inches);
//writeDebugStreamLine("Done with driveForward();");
}
void driveForward(float inches, int power) { // Overloaded function: 2 parameters
while(robotPaused) {}
if(menu.returnNow) return;
beginDrivingForward(power);
stopAfterDistance(inches);
}
void driveBackward() { // Overloaded function: 0 parameters
while(robotPaused) {}
if(menu.returnNow) return;
// writeDebugStreamLine("In driveBackward();");
// writeDebugStreamLine( "\n--------------\ndriveBackward();");
beginDrivingBackward(driveMotors.powerLevel);
}
void driveBackward(float inches) { // Overloaded function: 1 parameter
while(robotPaused) {}
if(menu.returnNow) return;
// writeDebugStreamLine( "\n--------------\ndriveBackward(%6.2f);", inches);
beginDrivingBackward(driveMotors.powerLevel);
stopAfterDistance(inches);
}
void driveBackward(float inches, int power) { // Overloaded function: 2 parameters
while(robotPaused) {}
if(menu.returnNow) return;
writeDebugStreamLine( "\n--------------\ndriveBackward(%6.2f , %d);", inches, power);
beginDrivingBackward(power);
stopAfterDistance(inches);
}
void followLine(int thisSide, float inches) {
while(robotPaused) {}
if(menu.returnNow) return;
followLine(thisSide);
stopAfterDistance(inches);
}
void followWall(int thisSide) { // Overloaded function: 1 parameters
while(robotPaused) {}
if(menu.returnNow) return;
beginFollowingWall(thisSide, driveMotors.powerLevel);
}
void followWall(int thisSide, float inches) { // Overloaded function: 2 parameters
while(robotPaused) {}
if(menu.returnNow) return;
beginFollowingWall(thisSide, driveMotors.powerLevel);
stopAfterDistance(inches);
}
void followWall(int thisSide, float inches, int power) { // Overloaded function: 3 parameters
while(robotPaused) {}
if(menu.returnNow) return;
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) {
while(robotPaused) {}
if(menu.returnNow) return;
gradualTurnClockwise(turnDegrees, turnRadiusIN, driveMotors.powerLevel);
}
void gradualTurnCounterClockwise(float turnDegrees, float turnRadiusIN) {
while(robotPaused) {}
if(menu.returnNow) return;
gradualTurnCounterClockwise(turnDegrees, turnRadiusIN, driveMotors.powerLevel);
}
void swingTurnClockwise(float turnDegrees) { // Overloaded function: 1 parameter
while(robotPaused) {}
if(menu.returnNow) return;
swingTurnClockwise(turnDegrees, driveMotors.powerLevel);
}
void swingTurnCounterClockwise(float turnDegrees) {// Overloaded function: 1 parameter
while(robotPaused) {}
if(menu.returnNow) return;
swingTurnCounterClockwise(turnDegrees, driveMotors.powerLevel);
}
void swingTurn(int dirRotation, float turnDegrees, int tempPower) { // new function: swingTurn()
while(robotPaused) {}
if(menu.returnNow) return;
clearDisplay(); display(2, "Point Turn");
if(dirRotation == COUNTERCLOCKWISE) {
display(3, "Counter Clockwise");
swingTurnCounterClockwise(turnDegrees, tempPower);
} else {
display(3, "Clockwise");
swingTurnClockwise(turnDegrees, tempPower);
}
}
void swingTurn(int dirRotation, float turnDegrees) { // Overloaded function: 2 parameters
while(robotPaused) {}
if(menu.returnNow) return;
swingTurn(dirRotation, turnDegrees, driveMotors.powerLevel);
}
void pointTurnClockwise(float turnDegrees) { pointTurnClockwise(turnDegrees, driveMotors.powerLevel); }
void pointTurnCounterClockwise(float turnDegrees) { pointTurnCounterClockwise(turnDegrees, driveMotors.powerLevel); }
void pointTurn(int dirRotation, float turnDegrees, int tempPower) { // new function: pointTurn()
while(robotPaused) {}
if(menu.returnNow) return;
clearDisplay(); display(2, "Point Turn");
if(dirRotation == COUNTERCLOCKWISE) {
display(3, "Counter Clockwise");
pointTurnCounterClockwise(turnDegrees, tempPower);
} else {
display(3, "Clockwise");
pointTurnClockwise(turnDegrees, tempPower);
}
}
void pointTurn(int dirRotation, float turnDegrees) { // Overloaded function: 2 parameters
while(robotPaused) {}
if(menu.returnNow) return;
pointTurn(dirRotation, turnDegrees, driveMotors.powerLevel);
}
void accessory1Up() {
while(robotPaused) {}
if(menu.returnNow) return;
clearDisplay(); display(2, "Accessory1 Up");
if(accessoryMotors.motorNum[0].orientation == REGULAR) { moveAccessoryUp(0, -1, 25); } else { moveAccessoryDown(0, -1, 25); }
}
void accessory2Up() {
while(robotPaused) {}
if(menu.returnNow) return;
clearDisplay(); display(2, "Accessory2 Up");
if(accessoryMotors.motorNum[1].orientation == REGULAR) { moveAccessoryUp(1, -1, 25); } else { moveAccessoryDown(1, -1, 25); }
}
void accessory1Down() {
while(robotPaused) {}
if(menu.returnNow) return;
clearDisplay(); display(2, "Accessory1 Down");
if(accessoryMotors.motorNum[0].orientation == REGULAR) { moveAccessoryDown(0, -1, 25); } else { moveAccessoryUp(0, -1, 25); }
}
void accessory2Down() {
while(robotPaused) {}
if(menu.returnNow) return;
clearDisplay(); display(2, "Accessory2 Down");
if(accessoryMotors.motorNum[1].orientation == REGULAR) { moveAccessoryDown(1, -1, 25); } else { moveAccessoryUp(1, -1, 25); }
}
void accessory1Up(float turnDegrees) {
while(robotPaused) {}
if(menu.returnNow) return;
//clearDisplay(); display(2, "Accessory1 Up"); displayBigTextLine(6, " %3d degrees", turnDegrees);
if(accessoryMotors.motorNum[0].orientation == REGULAR) { moveAccessoryUp(0, turnDegrees, 25); } else { moveAccessoryDown(0, turnDegrees, 25); }
}
void accessory2Up(float turnDegrees) {
while(robotPaused) {}
if(menu.returnNow) return;
//clearDisplay(); display(2, "Accessory2 Up"); displayBigTextLine(6, " %3d degrees", turnDegrees);
if(accessoryMotors.motorNum[1].orientation == REGULAR) { moveAccessoryUp(1, turnDegrees, 25); } else { moveAccessoryDown(1, turnDegrees, 25); }
}
void accessory1Up(float turnDegrees, int tempPower) {
while(robotPaused) {}
if(menu.returnNow) return;
//clearDisplay(); display(2, "Accessory1 Up"); displayBigTextLine(6, " %3d degrees", turnDegrees);
if(accessoryMotors.motorNum[0].orientation == REGULAR) { moveAccessoryUp(0, turnDegrees, tempPower); } else { moveAccessoryDown(0, turnDegrees, tempPower); }
}
void accessory2Up(float turnDegrees, int tempPower) {
while(robotPaused) {}
if(menu.returnNow) return;
//clearDisplay(); display(2, "Accessory2 Up"); displayBigTextLine(6, " %3d degrees", turnDegrees);
if(accessoryMotors.motorNum[1].orientation == REGULAR) { moveAccessoryUp(1, turnDegrees, tempPower); } else { moveAccessoryDown(1, turnDegrees, tempPower); }
}
void accessory1Down(float turnDegrees) {
while(robotPaused) {}
if(menu.returnNow) return;
//clearDisplay(); display(2, "Accessory1 Down"); displayBigTextLine(6, " %3d degrees", turnDegrees);
if(accessoryMotors.motorNum[0].orientation == REGULAR) { moveAccessoryDown(0, turnDegrees, accessoryMotors.powerLevel); } else { moveAccessoryUp(0, turnDegrees, accessoryMotors.powerLevel); }
}
void accessory2Down(float turnDegrees) {
while(robotPaused) {}
if(menu.returnNow) return;
//clearDisplay(); display(2, "Accessory2 Down"); displayBigTextLine(3, " %3d degrees", turnDegrees);
if(accessoryMotors.motorNum[1].orientation == REGULAR) { moveAccessoryDown(1, turnDegrees, accessoryMotors.powerLevel); } else { moveAccessoryUp(1, turnDegrees, accessoryMotors.powerLevel); }
}
void accessory1Down(float turnDegrees, int tempPower) {
while(robotPaused) {}
if(menu.returnNow) return;
//clearDisplay(); display(2, "Accessory1 Down"); displayBigTextLine(3, " %3d degrees", turnDegrees);
if(accessoryMotors.motorNum[0].orientation == REGULAR) { moveAccessoryDown(0, turnDegrees, tempPower); } else { moveAccessoryUp(0, turnDegrees, tempPower); }
}
void accessory2Down(float turnDegrees, int tempPower) {
while(robotPaused) {}
if(menu.returnNow) return;
//clearDisplay(); display(2, "Accessory2 Down"); displayBigTextLine(6, " %3d degrees", turnDegrees);
if(accessoryMotors.motorNum[1].orientation == REGULAR) { moveAccessoryDown(1, turnDegrees, tempPower); } else { moveAccessoryUp(1, turnDegrees, tempPower); }
}
void accessory1ToDegree(float turnDegrees) {
while(robotPaused) {}
if(menu.returnNow) return;
//clearDisplay(); display(2, "Accessory1"); displayBigTextLine(6, "to degree %3d", turnDegrees);
accessoryToDegree(0, turnDegrees, accessoryMotors.powerLevel);
}
void accessory2ToDegree(float turnDegrees) {
while(robotPaused) {}
if(menu.returnNow) return;
//clearDisplay(); display(2, "Accessory2"); displayBigTextLine(6, "to degree %3d", turnDegrees);
accessoryToDegree(1, turnDegrees, accessoryMotors.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
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();
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(touchSensor1);
untilRelease();
untilSonarGreaterThan(0);
waitForTouchSensor();
pause();
followLine(LEFTSIDE);
lineFollower.sideOfLine = MIDDLE; // Initialized here only to avoid a compiler warning that they are not used anywhere else.
lineFollower.sideOfLine = RIGHTSIDE;
lineFollower.sideOfLine = LEFTSIDE;
}
}
