/**
* Detects a mask of white borders around a starting point.
* The result is returned via call-by-reference parameters left, top, right, bottom.
*
* @return the detected mask in left, top, right, bottom; or -1, -1, -1, -1 if no mask could be detected
*/
static bool detectMask(int startX, int startY, int maskScanDirections, int maskScanSize[DIRECTIONS_COUNT], int maskScanDepth[DIRECTIONS_COUNT], int maskScanStep[DIRECTIONS_COUNT], float maskScanThreshold[DIRECTIONS_COUNT], int maskScanMinimum[DIMENSIONS_COUNT], int maskScanMaximum[DIMENSIONS_COUNT], int* left, int* top, int* right, int* bottom, struct IMAGE* image) {
int width;
int height;
int half[DIRECTIONS_COUNT];
bool success;
half[HORIZONTAL] = maskScanSize[HORIZONTAL] / 2;
half[VERTICAL] = maskScanSize[VERTICAL] / 2;
if ((maskScanDirections & 1<<HORIZONTAL) != 0) {
*left = startX - maskScanStep[HORIZONTAL] * detectEdge(startX, startY, -maskScanStep[HORIZONTAL], 0, maskScanSize[HORIZONTAL], maskScanDepth[HORIZONTAL], maskScanThreshold[HORIZONTAL], image) - half[HORIZONTAL];
*right = startX + maskScanStep[HORIZONTAL] * detectEdge(startX, startY, maskScanStep[HORIZONTAL], 0, maskScanSize[HORIZONTAL], maskScanDepth[HORIZONTAL], maskScanThreshold[HORIZONTAL], image) + half[HORIZONTAL];
} else { // full range of sheet
*left = 0;
*right = image->width - 1;
}
if ((maskScanDirections & 1<<VERTICAL) != 0) {
*top = startY - maskScanStep[VERTICAL] * detectEdge(startX, startY, 0, -maskScanStep[VERTICAL], maskScanSize[VERTICAL], maskScanDepth[VERTICAL], maskScanThreshold[VERTICAL], image) - half[VERTICAL];
*bottom = startY + maskScanStep[VERTICAL] * detectEdge(startX, startY, 0, maskScanStep[VERTICAL], maskScanSize[VERTICAL], maskScanDepth[VERTICAL], maskScanThreshold[VERTICAL], image) + half[VERTICAL];
} else { // full range of sheet
*top = 0;
*bottom = image->height - 1;
}
// if below minimum or above maximum, set to maximum
width = *right - *left;
height = *bottom - *top;
success = true;
if ( ((maskScanMinimum[WIDTH] != -1) && (width < maskScanMinimum[WIDTH])) || ((maskScanMaximum[WIDTH] != -1) && (width > maskScanMaximum[WIDTH])) ) {
width = maskScanMaximum[WIDTH] / 2;
*left = startX - width;
*right = startX + width;
success = false;;
}
if ( ((maskScanMinimum[HEIGHT] != -1) && (height < maskScanMinimum[HEIGHT])) || ((maskScanMaximum[HEIGHT] != -1) && (height > maskScanMaximum[HEIGHT])) ) {
height = maskScanMaximum[HEIGHT] / 2;
*top = startY - height;
*bottom = startY + height;
success = false;
}
return success;
}
Foam::pointHit Foam::controlMeshRefinement::findDiscontinuities
(
const linePointRef& l
) const
{
pointHit p(point::max);
const scalar tolSqr = sqr(1e-3);
const scalar secondDerivTolSqr = sqr(1e-3);
detectEdge
(
l.start(),
l.end(),
p,
tolSqr,
secondDerivTolSqr
);
return p;
}
void loop() {
// drive(1, false);
// return;
Robot.updateIR();
static int turnTestTime = 0;
static int turnTestTimeLast = 0;
static int turnTimeMod = 1;
switch(state) {
case DRIVE:
if(detectEdge()) {
Robot.motorsStop();
state = BACKUP;
time = 0;
//Robot.background(0, 255, 0);
}
else {
if(0 && random(0, 50) == 0) {
Robot.motorsStop();
state = TURN;
turnTestTime = 0;
turnTestTimeLast = 0;
turnTime = 1*random(5, 25);
turnDir = random(0, 1);
if(turnDir == 0)
turnDir = -1;
Robot.beep(BEEP_SIMPLE);
time = 0;
//Robot.background(255, 0, 0);
}
else
drive(1);
}
break;
case BACKUP:
if(time < 45) {
drive(-1, true);
}
else {
Robot.motorsStop();
state = TURN;
turnTestTime = 0;
turnTestTimeLast = 0;
turnTime = 1*random(5, 25);
turnDir = random(0, 1);
if(turnDir == 0)
turnDir = -1;
Robot.beep(BEEP_SIMPLE);
time = 0;
//Robot.background(255, 0, 0);
}
break;
case TURN:
if(time < turnTime) {
// if(turnTestTime >= 2*turnTestTimeLast + 1) {
// turnTestTimeLast = turnTestTime;
// turnTestTime = 0;
// turnTimeMod *= -1;
// } else
// turnTestTime++;
Robot.motorsWrite(255 * turnDir * turnTimeMod, -255 * turnDir * turnTimeMod);
}
else {
state = DRIVE;
Robot.motorsStop();
//Robot.background(0, 0, 255);
}
break;
}
time++;
delay(5);
}
