bool RobotFollowWhiteLineForDistance(float distance, bool avoidEnemies)
{
nxtDisplayCenteredTextLine(0, "folw line fo dist");
nxtDisplayCenteredTextLine(1, (string)distance);
PlaySound(soundUpwardTones);
MechanismElevatorTarget(kElevatorTargetLineFollowing);
_RobotZeroDriveEncoders();
bool success = true;
float brightnessRange = CurrentLineFollowingContext.lineBrightness - CurrentLineFollowingContext.surroundingBrightness;
float gain = kLineFollowerTurnRange / brightnessRange;
int targetEncoder = DriveMotorConvertDistanceToEncoder(distance);
nxtDisplayCenteredTextLine(4, (string)targetEncoder);
// go until we reach the distance
while ( ((nMotorEncoder[Left] + nMotorEncoder[Right]) / 2) < targetEncoder )
{
float left = LEFT_LIGHT_SENSOR();
float right = RIGHT_LIGHT_SENSOR();
float error = left - right;
nxtDisplayCenteredTextLine(0, (string)left);
nxtDisplayCenteredTextLine(1, (string)right);
float turn = error * gain;
motor[Left] = kLineFollowerMotorPower - turn;
motor[Right] = kLineFollowerMotorPower + turn;
if ( EnemyRobotDetected() && avoidEnemies )
{
success = false;
break;
}
}
RobotStop();
// update the position of the robot
int encoder = (nMotorEncoder[Left] + nMotorEncoder[Right]) / 2;
int distTravelled = DriveMotorConvertEncoderToDistance(encoder);
float orientation = CurrentRobotPosition.orientation;
CurrentRobotPosition.location.x += distTravelled * cos(orientation);
CurrentRobotPosition.location.y += distTravelled * sin(orientation);
return success;
}
BYTE BumperReaction(BYTE BLeft, BYTE BRight)
// Реакция на препятствие
{
if(!BLeft && !BRight) return 0;
RobotStop();
Flash(L_RED, 200);
Beep(1);
RobotGoBack();
delay_ms(750);
if(BLeft && BRight) RobotTurnRandom();
if(BLeft) RobotGoRight();
if(BRight) RobotGoLeft();
delay_ms(750);
RobotStop();
return 1;
}
void ProgramExit(void)
{
// Stop and close the 3BOT and the Oculus HMD.
RobotStop();
GRAPHICS_Stop();
// Timing results.
RobotTimingResults();
GraphicsTimingResults();
// Exit program.
exit(0);
}
bool RobotMoveDistance(float distance, bool avoidEnemies)
{
nxtDisplayCenteredTextLine(0, "moving distance");
_RobotZeroDriveEncoders();
int encoderPoints = DriveMotorConvertDistanceToEncoder(distance);
// start moving
motor[Left] = RobotMoveSpeed * SIGN(encoderPoints);
motor[Right] = motor[Left];
bool success;
while ( true )
{
bool left = false, right = false;
if ( MotorClose(Left, encoderPoints) )
motor[Left] = RobotMoveSpeed / 2 * SIGN(encoderPoints);
else
motor[Left] = RobotMoveSpeed * SIGN(encoderPoints);
if ( MotorClose(Right, encoderPoints) )
motor[Right] = RobotMoveSpeed / 2 * SIGN(encoderPoints);
else
motor[Right] = RobotMoveSpeed * SIGN(encoderPoints);
if ( abs(nMotorEncoder[Left]) > abs(encoderPoints) )
{
left = true;
motor[Left] = 0;
}
if ( abs(nMotorEncoder[Right]) > abs(encoderPoints) )
{
right = true;
motor[Right] = 0;
}
if ( left && right )
{
success = true;
break;
}
if ( avoidEnemies && EnemyRobotDetected() ) // if we see an enemy, stop
{
PlaySound(soundLowBuzz);
success = false;
RobotStop();
break;
}
}
// we're already stopped
// update position
float orientation = CurrentRobotPosition.orientation;
float travelled = DriveMotorConvertEncoderToDistance((nMotorEncoder[Left] + nMotorEncoder[Right]) / 2);
CurrentRobotPosition.location.x += travelled * cos(orientation);
CurrentRobotPosition.location.y += travelled * sin(orientation);
return success;
}
bool RobotFindWhiteLine() // returns true if it finds it
{
_RobotZeroDriveEncoders();
nxtDisplayCenteredTextLine(0, "finding line");
MechanismElevatorTarget(kElevatorTargetLineFollowing);
// initial values
CurrentLineFollowingContext.lineBrightness = 0;
CurrentLineFollowingContext.surroundingBrightness = 100;
float maxScanArc = kWhiteLineScanAngle * (kRobotWidth / 2);
int scanEncoder = DriveMotorConvertDistanceToEncoder(maxScanArc);
motor[Left] = kRobotLineScanPower;
motor[Right] = -kRobotLineScanPower;
while ( abs(nMotorEncoder[Left]) < abs(scanEncoder) )
{
_RecordLineBrightness(LEFT_LIGHT_SENSOR());
_RecordLineBrightness(RIGHT_LIGHT_SENSOR());
int contrast = CurrentLineFollowingContext.surroundingBrightness - CurrentLineFollowingContext.lineBrightness;
int edge = (CurrentLineFollowingContext.lineBrightness + CurrentLineFollowingContext.surroundingBrightness) / 2;
bool leftOnEdge = abs(LEFT_LIGHT_SENSOR() - edge) < kBrightnessEqualityThreshold;
if ( abs(contrast) > kMinLineSurroundingDifference && leftOnEdge )
{
RobotStop();
return true;
}
}
_RobotZeroDriveEncoders();
motor[Left] = -kRobotLineScanPower;
motor[Right] = kRobotLineScanPower;
while ( abs(nMotorEncoder[Right]) < abs(scanEncoder * 2) )
{
_RecordLineBrightness(LEFT_LIGHT_SENSOR());
_RecordLineBrightness(RIGHT_LIGHT_SENSOR());
int contrast = CurrentLineFollowingContext.surroundingBrightness - CurrentLineFollowingContext.lineBrightness;
int edge = (CurrentLineFollowingContext.lineBrightness + CurrentLineFollowingContext.surroundingBrightness) / 2;
bool rightOnEdge = abs(RIGHT_LIGHT_SENSOR() - edge) < kBrightnessEqualityThreshold;
if ( abs(contrast) > kMinLineSurroundingDifference && rightOnEdge )
{
RobotStop();
return true;
}
}
motor[Left] = kRobotLineScanPower;
motor[Right] = -kRobotLineScanPower;
// go back to the initial orientation
_RobotZeroDriveEncoders();
while ( abs(nMotorEncoder[Left]) < scanEncoder ) {}
RobotStop();
return false;
}
bool RobotFollowWhiteLineToEnd(bool avoidEnemies) // FIXME: error in this method!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
{
nxtDisplayCenteredTextLine(0, "following line");
MechanismElevatorTarget(kElevatorTargetLineFollowing);
_RobotZeroDriveEncoders();
float brightnessRange = CurrentLineFollowingContext.lineBrightness - CurrentLineFollowingContext.surroundingBrightness;
float gain = kLineFollowerTurnRange / brightnessRange;
bool success = true;
while ( true )
{
float left = LEFT_LIGHT_SENSOR();
float right = RIGHT_LIGHT_SENSOR();
float error = left - right;
bool same = abs(error) < kBrightnessEqualityThreshold;
bool notLine = ( ((left + right) / 2) - CurrentLineFollowingContext.surroundingBrightness) < kBrightnessEqualityThreshold;
if ( same && notLine )
{
break;
}
// abort if a robot is in the way
if ( EnemyRobotDetected() && avoidEnemies )
{
success = false;
break;
}
float turn = error * gain;
motor[Left] = kLineFollowerMotorPower - turn;
motor[Right] = kLineFollowerMotorPower + turn;
}
// stop
RobotStop();
// update the position of the robot
int encoder = (nMotorEncoder[Left] + nMotorEncoder[Right]) / 2;
int distance = DriveMotorConvertEncoderToDistance(encoder);
float orientation = CurrentRobotPosition.orientation;
CurrentRobotPosition.location.x += distance * cos(orientation);
CurrentRobotPosition.location.y += distance * sin(orientation);
return success;
}
